Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/Boards.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/Boards.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/Boards.h	(revision 224)
@@ -0,0 +1,740 @@
+/*
+  Boards.h - Hardware Abstraction Layer for Firmata library
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated December 19th, 2015
+*/
+
+#ifndef Firmata_Boards_h
+#define Firmata_Boards_h
+
+#include <inttypes.h>
+
+#if defined(ARDUINO) && ARDUINO >= 100
+#include "Arduino.h"	// for digitalRead, digitalWrite, etc
+#else
+#include "WProgram.h"
+#endif
+
+// Normally Servo.h must be included before Firmata.h (which then includes
+// this file).  If Servo.h wasn't included, this allows the code to still
+// compile, but without support for any Servos.  Hopefully that's what the
+// user intended by not including Servo.h
+#ifndef MAX_SERVOS
+#define MAX_SERVOS 0
+#endif
+
+/*
+    Firmata Hardware Abstraction Layer
+
+Firmata is built on top of the hardware abstraction functions of Arduino,
+specifically digitalWrite, digitalRead, analogWrite, analogRead, and 
+pinMode.  While these functions offer simple integer pin numbers, Firmata
+needs more information than is provided by Arduino.  This file provides
+all other hardware specific details.  To make Firmata support a new board,
+only this file should require editing.
+
+The key concept is every "pin" implemented by Firmata may be mapped to
+any pin as implemented by Arduino.  Usually a simple 1-to-1 mapping is
+best, but such mapping should not be assumed.  This hardware abstraction
+layer allows Firmata to implement any number of pins which map onto the
+Arduino implemented pins in almost any arbitrary way.
+
+
+General Constants:
+
+These constants provide basic information Firmata requires.
+
+TOTAL_PINS: The total number of pins Firmata implemented by Firmata.
+    Usually this will match the number of pins the Arduino functions
+    implement, including any pins pins capable of analog or digital.
+    However, Firmata may implement any number of pins.  For example,
+    on Arduino Mini with 8 analog inputs, 6 of these may be used
+    for digital functions, and 2 are analog only.  On such boards,
+    Firmata can implement more pins than Arduino's pinMode()
+    function, in order to accommodate those special pins.  The
+    Firmata protocol supports a maximum of 128 pins, so this
+    constant must not exceed 128.
+
+TOTAL_ANALOG_PINS: The total number of analog input pins implemented.
+    The Firmata protocol allows up to 16 analog inputs, accessed
+    using offsets 0 to 15.  Because Firmata presents the analog
+    inputs using different offsets than the actual pin numbers
+    (a legacy of Arduino's analogRead function, and the way the
+    analog input capable pins are physically labeled on all
+    Arduino boards), the total number of analog input signals
+    must be specified.  16 is the maximum.
+
+VERSION_BLINK_PIN: When Firmata starts up, it will blink the version
+    number.  This constant is the Arduino pin number where a
+    LED is connected.
+
+
+Pin Mapping Macros:
+
+These macros provide the mapping between pins as implemented by
+Firmata protocol and the actual pin numbers used by the Arduino
+functions.  Even though such mappings are often simple, pin
+numbers received by Firmata protocol should always be used as
+input to these macros, and the result of the macro should be
+used with with any Arduino function.
+
+When Firmata is extended to support a new pin mode or feature,
+a pair of macros should be added and used for all hardware
+access.  For simple 1:1 mapping, these macros add no actual
+overhead, yet their consistent use allows source code which
+uses them consistently to be easily adapted to all other boards
+with different requirements.
+
+IS_PIN_XXXX(pin): The IS_PIN macros resolve to true or non-zero
+    if a pin as implemented by Firmata corresponds to a pin
+    that actually implements the named feature.
+
+PIN_TO_XXXX(pin): The PIN_TO macros translate pin numbers as
+    implemented by Firmata to the pin numbers needed as inputs
+    to the Arduino functions.  The corresponding IS_PIN macro
+    should always be tested before using a PIN_TO macro, so
+    these macros only need to handle valid Firmata pin
+    numbers for the named feature.
+
+
+Port Access Inline Funtions:
+
+For efficiency, Firmata protocol provides access to digital
+input and output pins grouped by 8 bit ports.  When these
+groups of 8 correspond to actual 8 bit ports as implemented
+by the hardware, these inline functions can provide high
+speed direct port access.  Otherwise, a default implementation
+using 8 calls to digitalWrite or digitalRead is used.
+
+When porting Firmata to a new board, it is recommended to
+use the default functions first and focus only on the constants
+and macros above.  When those are working, if optimized port
+access is desired, these inline functions may be extended.
+The recommended approach defines a symbol indicating which
+optimization to use, and then conditional complication is
+used within these functions.
+
+readPort(port, bitmask):  Read an 8 bit port, returning the value.
+   port:    The port number, Firmata pins port*8 to port*8+7
+   bitmask: The actual pins to read, indicated by 1 bits.
+
+writePort(port, value, bitmask):  Write an 8 bit port.
+   port:    The port number, Firmata pins port*8 to port*8+7
+   value:   The 8 bit value to write
+   bitmask: The actual pins to write, indicated by 1 bits.
+*/
+
+/*==============================================================================
+ * Board Specific Configuration
+ *============================================================================*/
+
+#ifndef digitalPinHasPWM
+#define digitalPinHasPWM(p)     IS_PIN_DIGITAL(p)
+#endif
+
+// Arduino Duemilanove, Diecimila, and NG
+#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega328P__) || defined(__AVR_ATmega328__)
+#if defined(NUM_ANALOG_INPUTS) && NUM_ANALOG_INPUTS == 6
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#else
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              22 // 14 digital + 8 analog
+#endif
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) < 14 + TOTAL_ANALOG_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+#define ARDUINO_PINOUT_OPTIMIZE 1
+
+
+// Wiring (and board)
+#elif defined(WIRING)
+#define VERSION_BLINK_PIN       WLED
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= FIRST_ANALOG_PIN && (p) < (FIRST_ANALOG_PIN+TOTAL_ANALOG_PINS))
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - FIRST_ANALOG_PIN)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p) 
+
+
+// old Arduinos
+#elif defined(__AVR_ATmega8__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 19)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+#define ARDUINO_PINOUT_OPTIMIZE 1
+
+
+// Arduino Mega
+#elif defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+#define TOTAL_ANALOG_PINS       16
+#define TOTAL_PINS              70 // 54 digital + 16 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          19
+#define PIN_SERIAL1_TX          18
+#define PIN_SERIAL2_RX          17
+#define PIN_SERIAL2_TX          16
+#define PIN_SERIAL3_RX          15
+#define PIN_SERIAL3_TX          14
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) > 13 && (p) < 20)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Arduino DUE
+#elif defined(__SAM3X8E__)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              66 // 54 digital + 12 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          19
+#define PIN_SERIAL1_TX          18
+#define PIN_SERIAL2_RX          17
+#define PIN_SERIAL2_TX          16
+#define PIN_SERIAL3_RX          15
+#define PIN_SERIAL3_TX          14
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 20 || (p) == 21) // 70 71
+#define IS_PIN_SERIAL(p)        ((p) > 13 && (p) < 20)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Arduino M0 / M0 pro
+#elif defined(__SAMD21G18A__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              34 // 14 digital + 6 analog + 2 i2c + 3 spi. 34 is due to non linear enumeration of pins
+#define TOTAL_PORTS				3
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       (((p) >= 2 && (p) <= 13) || ((p)>=24 && (p)<=29))
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) <= 29)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define IS_PIN_SPI(p)			((p)== MOSI || (p) == MISO || (p == SCK))
+#define IS_PIN_SERIAL(p)		((p) == 0 || (p) == 1)
+#define PIN_SERIAL5_RX			0 //Serial5
+#define PIN_SERIAL5_TX			1 //Serial5
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 1.0
+#elif defined(__AVR_AT90USB162__)
+#define TOTAL_ANALOG_PINS       0
+#define TOTAL_PINS              21 // 21 digital + no analog
+#define VERSION_BLINK_PIN       6
+#define PIN_SERIAL1_RX          2
+#define PIN_SERIAL1_TX          3
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        (0)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           (0)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 2 || (p) == 3)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (0)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 2.0
+#elif defined(__AVR_ATmega32U4__) && defined(CORE_TEENSY)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              25 // 11 digital + 12 analog
+#define VERSION_BLINK_PIN       11
+#define PIN_SERIAL1_RX          7
+#define PIN_SERIAL1_TX          8
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 11 && (p) <= 22)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 5 || (p) == 6)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 7 || (p) == 8)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (((p) < 22) ? 21 - (p) : 11)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy 3.0, 3.1 and 3.2
+#elif defined(__MK20DX128__) || defined(__MK20DX256__)
+#define TOTAL_ANALOG_PINS       14
+#define TOTAL_PINS              38 // 24 digital + 10 analog-digital + 4 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define PIN_SERIAL2_RX          9
+#define PIN_SERIAL2_TX          10
+#define PIN_SERIAL3_RX          7
+#define PIN_SERIAL3_TX          8
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) <= 33)
+#define IS_PIN_ANALOG(p)        (((p) >= 14 && (p) <= 23) || ((p) >= 34 && (p) <= 38))
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SERIAL(p)        (((p) > 6 && (p) < 11) || ((p) == 0 || (p) == 1))
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (((p) <= 23) ? (p) - 14 : (p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy-LC
+#elif defined(__MKL26Z64__)
+#define TOTAL_ANALOG_PINS       13
+#define TOTAL_PINS              27 // 27 digital + 13 analog-digital
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define PIN_SERIAL2_RX          9
+#define PIN_SERIAL2_TX          10
+#define PIN_SERIAL3_RX          7
+#define PIN_SERIAL3_TX          8
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) <= 26)
+#define IS_PIN_ANALOG(p)        ((p) >= 14)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 18 || (p) == 19)
+#define IS_PIN_SERIAL(p)        (((p) > 6 && (p) < 11) || ((p) == 0 || (p) == 1))
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Teensy++ 1.0 and 2.0
+#elif defined(__AVR_AT90USB646__) || defined(__AVR_AT90USB1286__)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              46 // 38 digital + 8 analog
+#define VERSION_BLINK_PIN       6
+#define PIN_SERIAL1_RX          2
+#define PIN_SERIAL1_TX          3
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 38 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 0 || (p) == 1)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 2 || (p) == 3)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 38)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Leonardo
+#elif defined(__AVR_ATmega32U4__)
+#define TOTAL_ANALOG_PINS       12
+#define TOTAL_PINS              30 // 14 digital + 12 analog + 4 SPI (D14-D17 on ISP header)
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define IS_PIN_DIGITAL(p)       ((p) >= 0 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 18 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           ((p) == 3 || (p) == 5 || (p) == 6 || (p) == 9 || (p) == 10 || (p) == 11 || (p) == 13)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 2 || (p) == 3)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 0 || (p) == 1)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p) - 18
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Intel Galileo Board (gen 1 and 2) and Intel Edison
+#elif defined(ARDUINO_LINUX)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              20 // 14 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define PIN_SERIAL1_RX          0
+#define PIN_SERIAL1_TX          1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 19)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 19)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         (IS_PIN_DIGITAL(p) && (p) - 2 < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 0 || (p) == 1)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// RedBearLab BLE Nano with factory switch settings (S1 - S10)
+#elif defined(BLE_NANO)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              15 // 9 digital + 3 analog
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 14)
+#define IS_PIN_ANALOG(p)        ((p) == 8 || (p) == 9 || (p) == 10 || (p) == 11 || (p) == 12 || (p) == 14) //A0~A5
+#define IS_PIN_PWM(p)           ((p) == 3 || (p) == 5 || (p) == 6)
+#define IS_PIN_SERVO(p)         ((p) >= 2 && (p) <= 7)
+#define IS_PIN_I2C(p)           ((p) == SDA || (p) == SCL)
+#define IS_PIN_SPI(p)           ((p) == CS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 8)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Sanguino
+#elif defined(__AVR_ATmega644P__) || defined(__AVR_ATmega644__)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              32 // 24 digital + 8 analog
+#define VERSION_BLINK_PIN       0
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Illuminato
+#elif defined(__AVR_ATmega645__)
+#define TOTAL_ANALOG_PINS       6
+#define TOTAL_PINS              42 // 36 digital + 6 analog
+#define VERSION_BLINK_PIN       13
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) < TOTAL_PINS)
+#define IS_PIN_ANALOG(p)        ((p) >= 36 && (p) < TOTAL_PINS)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         ((p) >= 0 && (p) < MAX_SERVOS)
+#define IS_PIN_I2C(p)           ((p) == 4 || (p) == 5)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 36)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+
+// Pic32 chipKIT FubarinoSD
+#elif defined(_BOARD_FUBARINO_SD_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 15
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 45, All pins can be digital
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       1
+#define IS_PIN_ANALOG(p)        ((p) >= 30 && (p) <= 44)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 1 || (p) == 2)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (14 - (p - 30))
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT FubarinoMini
+// Note, FubarinoMini analog pin 20 will not function in Firmata as analog input due to limitation in analog mapping
+#elif defined(_BOARD_FUBARINO_MINI_)
+#define TOTAL_ANALOG_PINS       14 // We have to fake this because of the poor analog pin mapping planning in FubarinoMini
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 33
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) != 14 && (p) != 15 && (p) != 31 && (p) != 32)
+#define IS_PIN_ANALOG(p)        ((p) == 0 || ((p) >= 3 && (p) <= 13))
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 25 || (p) == 26)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT UNO32
+#elif defined(_BOARD_UNO_) && defined(__PIC32)  // NOTE: no _BOARD_UNO32_ to use
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS // 12
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 47 All pins can be digital
+#define MAX_SERVOS              NUM_DIGITAL_PINS // All pins can be servo with SoftPWMservo
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 45 || (p) == 46)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT DP32
+#elif defined(_BOARD_DP32_)
+#define TOTAL_ANALOG_PINS       15  // Really only has 9, but have to override because of mistake in variant file
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 19
+#define MAX_SERVOS              NUM_DIGITAL_PINS // All pins can be servo with SoftPWMservo
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       (((p) != 1) && ((p) != 4) && ((p) != 5) && ((p) != 15) && ((p) != 16))
+#define IS_PIN_ANALOG(p)        ((p) >= 6 && (p) <= 14)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 2 || (p) == 3)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        (p)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT uC32
+#elif defined(_BOARD_UC32_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 12
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 47 All pins can be digital
+#define MAX_SERVOS              NUM_DIGITAL_PINS // All pins can be servo with SoftPWMservo
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2)
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 45 || (p) == 46)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT WF32
+#elif defined(_BOARD_WF32_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS
+#define TOTAL_PINS              NUM_DIGITAL_PINS
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 49)     // Accounts for SD and WiFi dedicated pins
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 34 || (p) == 35)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 14)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT WiFire
+#elif defined(_BOARD_WIFIRE_)
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 14
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 71
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2 && (p) <= 47)     // Accounts for SD and WiFi dedicated pins
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 25)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 34 || (p) == 35)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) <= 25 ? ((p) - 14) : (p) - 36)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT MAX32
+#elif defined(_BOARD_MEGA_) && defined(__PIC32)  // NOTE: no _BOARD_MAX32_ to use
+#define TOTAL_ANALOG_PINS       NUM_ANALOG_PINS  // 16
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 87
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       ((p) >= 2)
+#define IS_PIN_ANALOG(p)        ((p) >= 54 && (p) <= 69)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 34 || (p) == 35)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 54)
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+
+// Pic32 chipKIT Pi
+#elif defined(_BOARD_CHIPKIT_PI_)
+#define TOTAL_ANALOG_PINS       16
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 19
+#define MAX_SERVOS              NUM_DIGITAL_PINS
+#define VERSION_BLINK_PIN       PIN_LED1
+#define IS_PIN_DIGITAL(p)       (((p) >= 2) && ((p) <= 3) || (((p) >= 8) && ((p) <= 13)) || (((p) >= 14) && ((p) <= 17)))
+#define IS_PIN_ANALOG(p)        ((p) >= 14 && (p) <= 17)
+#define IS_PIN_PWM(p)           IS_PIN_DIGITAL(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == 16 || (p) == 17)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) <= 15 ? (p) - 14 : (p) - 12)
+//#define PIN_TO_ANALOG(p)        (((p) <= 16) ? ((p) - 14) : ((p) - 16))
+#define PIN_TO_PWM(p)           (p)
+#define PIN_TO_SERVO(p)         (p)
+
+// Pinoccio Scout
+// Note: digital pins 9-16 are usable but not labeled on the board numerically.
+// SS=9, MOSI=10, MISO=11, SCK=12, RX1=13, TX1=14, SCL=15, SDA=16
+#elif defined(ARDUINO_PINOCCIO)
+#define TOTAL_ANALOG_PINS       8
+#define TOTAL_PINS              NUM_DIGITAL_PINS // 32
+#define VERSION_BLINK_PIN       23
+#define PIN_SERIAL1_RX          13
+#define PIN_SERIAL1_TX          14
+#define IS_PIN_DIGITAL(p)       (((p) >= 2) && ((p) <= 16)) || (((p) >= 24) && ((p) <= 31))
+#define IS_PIN_ANALOG(p)        ((p) >= 24 && (p) <= 31)
+#define IS_PIN_PWM(p)           digitalPinHasPWM(p)
+#define IS_PIN_SERVO(p)         IS_PIN_DIGITAL(p)
+#define IS_PIN_I2C(p)           ((p) == SCL || (p) == SDA)
+#define IS_PIN_SPI(p)           ((p) == SS || (p) == MOSI || (p) == MISO || (p) == SCK)
+#define IS_PIN_SERIAL(p)        ((p) == 13 || (p) == 14)
+#define PIN_TO_DIGITAL(p)       (p)
+#define PIN_TO_ANALOG(p)        ((p) - 24)
+#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
+#define PIN_TO_SERVO(p)         ((p) - 2)
+
+// anything else
+#else
+#error "Please edit Boards.h with a hardware abstraction for this board"
+#endif
+
+// as long this is not defined for all boards:
+#ifndef IS_PIN_SPI
+#define IS_PIN_SPI(p)           0
+#endif
+
+#ifndef IS_PIN_SERIAL
+#define IS_PIN_SERIAL(p)        0
+#endif
+
+/*==============================================================================
+ * readPort() - Read an 8 bit port
+ *============================================================================*/
+
+static inline unsigned char readPort(byte, byte) __attribute__((always_inline, unused));
+static inline unsigned char readPort(byte port, byte bitmask)
+{
+#if defined(ARDUINO_PINOUT_OPTIMIZE)
+	if (port == 0) return (PIND & 0xFC) & bitmask; // ignore Rx/Tx 0/1
+	if (port == 1) return ((PINB & 0x3F) | ((PINC & 0x03) << 6)) & bitmask;
+	if (port == 2) return ((PINC & 0x3C) >> 2) & bitmask;
+	return 0;
+#else
+	unsigned char out=0, pin=port*8;
+	if (IS_PIN_DIGITAL(pin+0) && (bitmask & 0x01) && digitalRead(PIN_TO_DIGITAL(pin+0))) out |= 0x01;
+	if (IS_PIN_DIGITAL(pin+1) && (bitmask & 0x02) && digitalRead(PIN_TO_DIGITAL(pin+1))) out |= 0x02;
+	if (IS_PIN_DIGITAL(pin+2) && (bitmask & 0x04) && digitalRead(PIN_TO_DIGITAL(pin+2))) out |= 0x04;
+	if (IS_PIN_DIGITAL(pin+3) && (bitmask & 0x08) && digitalRead(PIN_TO_DIGITAL(pin+3))) out |= 0x08;
+	if (IS_PIN_DIGITAL(pin+4) && (bitmask & 0x10) && digitalRead(PIN_TO_DIGITAL(pin+4))) out |= 0x10;
+	if (IS_PIN_DIGITAL(pin+5) && (bitmask & 0x20) && digitalRead(PIN_TO_DIGITAL(pin+5))) out |= 0x20;
+	if (IS_PIN_DIGITAL(pin+6) && (bitmask & 0x40) && digitalRead(PIN_TO_DIGITAL(pin+6))) out |= 0x40;
+	if (IS_PIN_DIGITAL(pin+7) && (bitmask & 0x80) && digitalRead(PIN_TO_DIGITAL(pin+7))) out |= 0x80;
+	return out;
+#endif
+}
+
+/*==============================================================================
+ * writePort() - Write an 8 bit port, only touch pins specified by a bitmask
+ *============================================================================*/
+
+static inline unsigned char writePort(byte, byte, byte) __attribute__((always_inline, unused));
+static inline unsigned char writePort(byte port, byte value, byte bitmask)
+{
+#if defined(ARDUINO_PINOUT_OPTIMIZE)
+	if (port == 0) {
+		bitmask = bitmask & 0xFC;  // do not touch Tx & Rx pins
+		byte valD = value & bitmask;
+		byte maskD = ~bitmask;
+		cli();
+		PORTD = (PORTD & maskD) | valD;
+		sei();
+	} else if (port == 1) {
+		byte valB = (value & bitmask) & 0x3F;
+		byte valC = (value & bitmask) >> 6;
+		byte maskB = ~(bitmask & 0x3F);
+		byte maskC = ~((bitmask & 0xC0) >> 6);
+		cli();
+		PORTB = (PORTB & maskB) | valB;
+		PORTC = (PORTC & maskC) | valC;
+		sei();
+	} else if (port == 2) {
+		bitmask = bitmask & 0x0F;
+		byte valC = (value & bitmask) << 2;
+		byte maskC = ~(bitmask << 2);
+		cli();
+		PORTC = (PORTC & maskC) | valC;
+		sei();
+	}
+#else
+	byte pin=port*8;
+	if ((bitmask & 0x01)) digitalWrite(PIN_TO_DIGITAL(pin+0), (value & 0x01));
+	if ((bitmask & 0x02)) digitalWrite(PIN_TO_DIGITAL(pin+1), (value & 0x02));
+	if ((bitmask & 0x04)) digitalWrite(PIN_TO_DIGITAL(pin+2), (value & 0x04));
+	if ((bitmask & 0x08)) digitalWrite(PIN_TO_DIGITAL(pin+3), (value & 0x08));
+	if ((bitmask & 0x10)) digitalWrite(PIN_TO_DIGITAL(pin+4), (value & 0x10));
+	if ((bitmask & 0x20)) digitalWrite(PIN_TO_DIGITAL(pin+5), (value & 0x20));
+	if ((bitmask & 0x40)) digitalWrite(PIN_TO_DIGITAL(pin+6), (value & 0x40));
+	if ((bitmask & 0x80)) digitalWrite(PIN_TO_DIGITAL(pin+7), (value & 0x80));
+
+#endif
+}
+
+
+
+
+#ifndef TOTAL_PORTS
+#define TOTAL_PORTS             ((TOTAL_PINS + 7) / 8)
+#endif
+
+
+#endif /* Firmata_Boards_h */
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.cpp	(revision 224)
@@ -0,0 +1,668 @@
+/*
+  Firmata.cpp - Firmata library v2.5.2 - 2016-2-15
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+//******************************************************************************
+//* Includes
+//******************************************************************************
+
+#include "Firmata.h"
+#include "HardwareSerial.h"
+
+extern "C" {
+#include <string.h>
+#include <stdlib.h>
+}
+
+//******************************************************************************
+//* Support Functions
+//******************************************************************************
+
+/**
+ * Split a 16-bit byte into two 7-bit values and write each value.
+ * @param value The 16-bit value to be split and written separately.
+ */
+void FirmataClass::sendValueAsTwo7bitBytes(int value)
+{
+  FirmataStream->write(value & 0x7F); // LSB
+  FirmataStream->write(value >> 7 & 0x7F); // MSB
+}
+
+/**
+ * A helper method to write the beginning of a Sysex message transmission.
+ */
+void FirmataClass::startSysex(void)
+{
+  FirmataStream->write(START_SYSEX);
+}
+
+/**
+ * A helper method to write the end of a Sysex message transmission.
+ */
+void FirmataClass::endSysex(void)
+{
+  FirmataStream->write(END_SYSEX);
+}
+
+//******************************************************************************
+//* Constructors
+//******************************************************************************
+
+/**
+ * The Firmata class.
+ * An instance named "Firmata" is created automatically for the user.
+ */
+FirmataClass::FirmataClass()
+{
+  firmwareVersionCount = 0;
+  firmwareVersionVector = 0;
+  systemReset();
+}
+
+//******************************************************************************
+//* Public Methods
+//******************************************************************************
+
+/**
+ * Initialize the default Serial transport at the default baud of 57600.
+ */
+void FirmataClass::begin(void)
+{
+  begin(57600);
+}
+
+/**
+ * Initialize the default Serial transport and override the default baud.
+ * Sends the protocol version to the host application followed by the firmware version and name.
+ * blinkVersion is also called. To skip the call to blinkVersion, call Firmata.disableBlinkVersion()
+ * before calling Firmata.begin(baud).
+ * @param speed The baud to use. 57600 baud is the default value.
+ */
+void FirmataClass::begin(long speed)
+{
+  Serial.begin(speed);
+  FirmataStream = &Serial;
+  blinkVersion();
+  printVersion();         // send the protocol version
+  printFirmwareVersion(); // send the firmware name and version
+}
+
+/**
+ * Reassign the Firmata stream transport.
+ * @param s A reference to the Stream transport object. This can be any type of
+ * transport that implements the Stream interface. Some examples include Ethernet, WiFi
+ * and other UARTs on the board (Serial1, Serial2, etc).
+ */
+void FirmataClass::begin(Stream &s)
+{
+  FirmataStream = &s;
+  // do not call blinkVersion() here because some hardware such as the
+  // Ethernet shield use pin 13
+  printVersion();
+  printFirmwareVersion();
+}
+
+/**
+ * Send the Firmata protocol version to the Firmata host application.
+ */
+void FirmataClass::printVersion(void)
+{	
+  FirmataStream->write(REPORT_VERSION);
+  FirmataStream->write(FIRMATA_PROTOCOL_MAJOR_VERSION);
+  FirmataStream->write(FIRMATA_PROTOCOL_MINOR_VERSION);
+}
+
+/**
+ * Blink the Firmata protocol version to the onboard LEDs (if the board has an onboard LED).
+ * If VERSION_BLINK_PIN is not defined in Boards.h for a particular board, then this method
+ * does nothing.
+ * The first series of flashes indicates the firmware major version (2 flashes = 2).
+ * The second series of flashes indicates the firmware minor version (5 flashes = 5).
+ */
+void FirmataClass::blinkVersion(void)
+{
+#if defined(VERSION_BLINK_PIN)
+  if (blinkVersionDisabled) return;
+  // flash the pin with the protocol version
+  pinMode(VERSION_BLINK_PIN, OUTPUT);
+  strobeBlinkPin(VERSION_BLINK_PIN, FIRMATA_FIRMWARE_MAJOR_VERSION, 40, 210);
+  delay(250);
+  strobeBlinkPin(VERSION_BLINK_PIN, FIRMATA_FIRMWARE_MINOR_VERSION, 40, 210);
+  delay(125);
+#endif
+}
+
+/**
+ * Provides a means to disable the version blink sequence on the onboard LED, trimming startup
+ * time by a couple of seconds.
+ * Call this before Firmata.begin(). It only applies when using the default Serial transport.
+ */
+void FirmataClass::disableBlinkVersion()
+{
+  blinkVersionDisabled = true;
+}
+
+/**
+ * Sends the firmware name and version to the Firmata host application. The major and minor version
+ * numbers are the first 2 bytes in the message. The following bytes are the characters of the
+ * firmware name.
+ */
+void FirmataClass::printFirmwareVersion(void)
+{
+  byte i;
+
+  if (firmwareVersionCount) { // make sure that the name has been set before reporting
+    startSysex();
+    FirmataStream->write(REPORT_FIRMWARE);
+    FirmataStream->write(firmwareVersionVector[0]); // major version number
+    FirmataStream->write(firmwareVersionVector[1]); // minor version number
+    for (i = 2; i < firmwareVersionCount; ++i) {
+      sendValueAsTwo7bitBytes(firmwareVersionVector[i]);
+    }
+    endSysex();
+  }
+}
+
+/**
+ * Sets the name and version of the firmware. This is not the same version as the Firmata protocol
+ * (although at times the firmware version and protocol version may be the same number).
+ * @param name A pointer to the name char array
+ * @param major The major version number
+ * @param minor The minor version number
+ */
+void FirmataClass::setFirmwareNameAndVersion(const char *name, byte major, byte minor)
+{
+  const char *firmwareName;
+  const char *extension;
+
+  // parse out ".cpp" and "applet/" that comes from using __FILE__
+  extension = strstr(name, ".cpp");
+  firmwareName = strrchr(name, '/');
+
+  if (!firmwareName) {
+    // windows
+    firmwareName = strrchr(name, '\\');
+  }
+  if (!firmwareName) {
+    // user passed firmware name
+    firmwareName = name;
+  } else {
+    firmwareName ++;
+  }
+
+  if (!extension) {
+    firmwareVersionCount = strlen(firmwareName) + 2;
+  } else {
+    firmwareVersionCount = extension - firmwareName + 2;
+  }
+
+  // in case anyone calls setFirmwareNameAndVersion more than once
+  free(firmwareVersionVector);
+
+  firmwareVersionVector = (byte *) malloc(firmwareVersionCount + 1);
+  firmwareVersionVector[firmwareVersionCount] = 0;
+  firmwareVersionVector[0] = major;
+  firmwareVersionVector[1] = minor;
+  strncpy((char *)firmwareVersionVector + 2, firmwareName, firmwareVersionCount - 2);
+}
+
+//------------------------------------------------------------------------------
+// Serial Receive Handling
+
+/**
+ * A wrapper for Stream::available()
+ * @return The number of bytes remaining in the input stream buffer.
+ */
+int FirmataClass::available(void)
+{
+  return FirmataStream->available();
+}
+
+/**
+ * Process incoming sysex messages. Handles REPORT_FIRMWARE and STRING_DATA internally.
+ * Calls callback function for STRING_DATA and all other sysex messages.
+ * @private
+ */
+void FirmataClass::processSysexMessage(void)
+{
+  switch (storedInputData[0]) { //first byte in buffer is command
+    case REPORT_FIRMWARE:
+      printFirmwareVersion();
+      break;
+    case STRING_DATA:
+      if (currentStringCallback) {
+        byte bufferLength = (sysexBytesRead - 1) / 2;
+        byte i = 1;
+        byte j = 0;
+        while (j < bufferLength) {
+          // The string length will only be at most half the size of the
+          // stored input buffer so we can decode the string within the buffer.
+          storedInputData[j] = storedInputData[i];
+          i++;
+          storedInputData[j] += (storedInputData[i] << 7);
+          i++;
+          j++;
+        }
+        // Make sure string is null terminated. This may be the case for data
+        // coming from client libraries in languages that don't null terminate
+        // strings.
+        if (storedInputData[j - 1] != '\0') {
+          storedInputData[j] = '\0';
+        }
+        (*currentStringCallback)((char *)&storedInputData[0]);
+      }
+      break;
+    default:
+      if (currentSysexCallback)
+        (*currentSysexCallback)(storedInputData[0], sysexBytesRead - 1, storedInputData + 1);
+  }
+}
+
+/**
+ * Read a single int from the input stream. If the value is not = -1, pass it on to parse(byte)
+ */
+void FirmataClass::processInput(void)
+{
+  int inputData = FirmataStream->read(); // this is 'int' to handle -1 when no data
+  if (inputData != -1) {
+    parse(inputData);
+  }
+}
+
+/**
+ * Parse data from the input stream.
+ * @param inputData A single byte to be added to the parser.
+ */
+void FirmataClass::parse(byte inputData)
+{
+  int command;
+  if (parsingSysex) {
+    if (inputData == END_SYSEX) {
+      //stop sysex byte
+      parsingSysex = false;
+      //fire off handler function
+      processSysexMessage();
+    } else {
+      //normal data byte - add to buffer
+      storedInputData[sysexBytesRead] = inputData;
+      sysexBytesRead++;
+		
+    }
+  } else if ( (waitForData > 0) && (inputData < 128) ) {
+    waitForData--;
+    storedInputData[waitForData] = inputData;
+    if ( (waitForData == 0) && executeMultiByteCommand ) { // got the whole message
+      switch (executeMultiByteCommand) {
+        case ANALOG_MESSAGE:
+          if (currentAnalogCallback) {
+            (*currentAnalogCallback)(multiByteChannel,
+                                     (storedInputData[0] << 7)
+                                     + storedInputData[1]);
+          }
+          break;
+        case DIGITAL_MESSAGE:
+          if (currentDigitalCallback) {
+            (*currentDigitalCallback)(multiByteChannel,
+                                      (storedInputData[0] << 7)
+                                      + storedInputData[1]);
+          }
+          break;
+         case SET_PIN_MODE:
+          if (currentPinModeCallback)
+            (*currentPinModeCallback)(storedInputData[1], storedInputData[0]);
+          break;
+        case SET_DIGITAL_PIN_VALUE:
+          if (currentPinValueCallback)
+            (*currentPinValueCallback)(storedInputData[1], storedInputData[0]);
+          break;
+        case REPORT_ANALOG:
+          if (currentReportAnalogCallback)
+            (*currentReportAnalogCallback)(multiByteChannel, storedInputData[0]);
+          break;
+        case REPORT_DIGITAL:
+          if (currentReportDigitalCallback)
+            (*currentReportDigitalCallback)(multiByteChannel, storedInputData[0]);
+          break;
+      }
+      executeMultiByteCommand = 0;
+    }
+  } else {
+    // remove channel info from command byte if less than 0xF0
+    if (inputData < 0xF0) {
+      command = inputData & 0xF0;
+      multiByteChannel = inputData & 0x0F;
+    } else {
+      command = inputData;
+      // commands in the 0xF* range don't use channel data
+    }
+    switch (command) {
+      case ANALOG_MESSAGE:
+      case DIGITAL_MESSAGE:
+      case SET_PIN_MODE:
+      case SET_DIGITAL_PIN_VALUE:
+        waitForData = 2; // two data bytes needed
+        executeMultiByteCommand = command;
+        break;
+      case REPORT_ANALOG:
+      case REPORT_DIGITAL:
+        waitForData = 1; // one data byte needed
+        executeMultiByteCommand = command;
+        break;
+      case START_SYSEX:
+        parsingSysex = true;
+        sysexBytesRead = 0;
+        break;
+      case SYSTEM_RESET:
+        systemReset();
+        break;
+      case REPORT_VERSION:
+        Firmata.printVersion();
+        break;
+    }
+  }
+}
+
+/**
+ * @return Returns true if the parser is actively parsing data.
+ */
+boolean FirmataClass::isParsingMessage(void)
+{
+  return (waitForData > 0 || parsingSysex);
+}
+
+//------------------------------------------------------------------------------
+// Output Stream Handling
+
+/**
+ * Send an analog message to the Firmata host application. The range of pins is limited to [0..15]
+ * when using the ANALOG_MESSAGE. The maximum value of the ANALOG_MESSAGE is limited to 14 bits
+ * (16384). To increase the pin range or value, see the documentation for the EXTENDED_ANALOG
+ * message.
+ * @param pin The analog pin to send the value of (limited to pins 0 - 15).
+ * @param value The value of the analog pin (0 - 1024 for 10-bit analog, 0 - 4096 for 12-bit, etc).
+ * The maximum value is 14-bits (16384).
+ */
+void FirmataClass::sendAnalog(byte pin, int value)
+{
+  // pin can only be 0-15, so chop higher bits
+  FirmataStream->write(ANALOG_MESSAGE | (pin & 0xF));
+  sendValueAsTwo7bitBytes(value);
+}
+
+/* (intentionally left out asterix here)
+ * STUB - NOT IMPLEMENTED
+ * Send a single digital pin value to the Firmata host application.
+ * @param pin The digital pin to send the value of.
+ * @param value The value of the pin.
+ */
+void FirmataClass::sendDigital(byte pin, int value)
+{
+  /* TODO add single pin digital messages to the protocol, this needs to
+   * track the last digital data sent so that it can be sure to change just
+   * one bit in the packet.  This is complicated by the fact that the
+   * numbering of the pins will probably differ on Arduino, Wiring, and
+   * other boards.
+   */
+
+  // TODO: the digital message should not be sent on the serial port every
+  // time sendDigital() is called.  Instead, it should add it to an int
+  // which will be sent on a schedule.  If a pin changes more than once
+  // before the digital message is sent on the serial port, it should send a
+  // digital message for each change.
+
+  //    if(value == 0)
+  //        sendDigitalPortPair();
+}
+
+
+/**
+ * Send an 8-bit port in a single digital message (protocol v2 and later).
+ * Send 14-bits in a single digital message (protocol v1).
+ * @param portNumber The port number to send. Note that this is not the same as a "port" on the
+ * physical microcontroller. Ports are defined in order per every 8 pins in ascending order
+ * of the Arduino digital pin numbering scheme. Port 0 = pins D0 - D7, port 1 = pins D8 - D15, etc.
+ * @param portData The value of the port. The value of each pin in the port is represented by a bit.
+ */
+void FirmataClass::sendDigitalPort(byte portNumber, int portData)
+{
+  FirmataStream->write(DIGITAL_MESSAGE | (portNumber & 0xF));
+  FirmataStream->write((byte)portData % 128); // Tx bits 0-6 (protocol v1 and higher)
+  FirmataStream->write(portData >> 7);  // Tx bits 7-13 (bit 7 only for protocol v2 and higher)
+}
+
+/**
+ * Send a sysex message where all values after the command byte are packet as 2 7-bit bytes
+ * (this is not always the case so this function is not always used to send sysex messages).
+ * @param command The sysex command byte.
+ * @param bytec The number of data bytes in the message (excludes start, command and end bytes).
+ * @param bytev A pointer to the array of data bytes to send in the message.
+ */
+void FirmataClass::sendSysex(byte command, byte bytec, byte *bytev)
+{
+  byte i;
+  startSysex();
+  FirmataStream->write(command);
+  for (i = 0; i < bytec; i++) {
+    sendValueAsTwo7bitBytes(bytev[i]);
+  }
+  endSysex();
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param command Must be STRING_DATA
+ * @param string A pointer to the char string
+ */
+void FirmataClass::sendString(byte command, const char *string)
+{
+  if (command == STRING_DATA) {
+    sendSysex(command, strlen(string), (byte *)string);
+  }
+}
+
+/**
+ * Send a string to the Firmata host application.
+ * @param string A pointer to the char string
+ */
+void FirmataClass::sendString(const char *string)
+{
+  sendString(STRING_DATA, string);
+}
+
+/**
+ * A wrapper for Stream::available().
+ * Write a single byte to the output stream.
+ * @param c The byte to be written.
+ */
+void FirmataClass::write(byte c)
+{
+  FirmataStream->write(c);
+}
+
+/**
+ * Attach a generic sysex callback function to a command (options are: ANALOG_MESSAGE,
+ * DIGITAL_MESSAGE, REPORT_ANALOG, REPORT DIGITAL, SET_PIN_MODE and SET_DIGITAL_PIN_VALUE).
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the callback function to attach.
+ */
+void FirmataClass::attach(byte command, callbackFunction newFunction)
+{
+  switch (command) {
+    case ANALOG_MESSAGE: currentAnalogCallback = newFunction; break;
+    case DIGITAL_MESSAGE: currentDigitalCallback = newFunction; break;
+    case REPORT_ANALOG: currentReportAnalogCallback = newFunction; break;
+    case REPORT_DIGITAL: currentReportDigitalCallback = newFunction; break;
+    case SET_PIN_MODE: currentPinModeCallback = newFunction; break;
+    case SET_DIGITAL_PIN_VALUE: currentPinValueCallback = newFunction; break;
+  }
+}
+
+/**
+ * Attach a callback function for the SYSTEM_RESET command.
+ * @param command Must be set to SYSTEM_RESET or it will be ignored.
+ * @param newFunction A reference to the system reset callback function to attach.
+ */
+void FirmataClass::attach(byte command, systemResetCallbackFunction newFunction)
+{
+  switch (command) {
+    case SYSTEM_RESET: currentSystemResetCallback = newFunction; break;
+  }
+}
+
+/**
+ * Attach a callback function for the STRING_DATA command.
+ * @param command Must be set to STRING_DATA or it will be ignored.
+ * @param newFunction A reference to the string callback function to attach.
+ */
+void FirmataClass::attach(byte command, stringCallbackFunction newFunction)
+{
+  switch (command) {
+    case STRING_DATA: currentStringCallback = newFunction; break;
+  }
+}
+
+/**
+ * Attach a generic sysex callback function to sysex command.
+ * @param command The ID of the command to attach a callback function to.
+ * @param newFunction A reference to the sysex callback function to attach.
+ */
+void FirmataClass::attach(byte command, sysexCallbackFunction newFunction)
+{
+  currentSysexCallback = newFunction;
+}
+
+/**
+ * Detach a callback function for a specified command (such as SYSTEM_RESET, STRING_DATA,
+ * ANALOG_MESSAGE, DIGITAL_MESSAGE, etc).
+ * @param command The ID of the command to detatch the callback function from.
+ */
+void FirmataClass::detach(byte command)
+{
+  switch (command) {
+    case SYSTEM_RESET: currentSystemResetCallback = NULL; break;
+    case STRING_DATA: currentStringCallback = NULL; break;
+    case START_SYSEX: currentSysexCallback = NULL; break;
+    default:
+      attach(command, (callbackFunction)NULL);
+  }
+}
+
+/**
+ * @param pin The pin to get the configuration of.
+ * @return The configuration of the specified pin.
+ */
+byte FirmataClass::getPinMode(byte pin)
+{
+  return pinConfig[pin];
+}
+
+/**
+ * Set the pin mode/configuration. The pin configuration (or mode) in Firmata represents the
+ * current function of the pin. Examples are digital input or output, analog input, pwm, i2c,
+ * serial (uart), etc.
+ * @param pin The pin to configure.
+ * @param config The configuration value for the specified pin.
+ */
+void FirmataClass::setPinMode(byte pin, byte config)
+{
+  if (pinConfig[pin] == PIN_MODE_IGNORE)
+    return;
+
+  pinConfig[pin] = config;
+}
+
+/**
+ * @param pin The pin to get the state of.
+ * @return The state of the specified pin.
+ */
+int FirmataClass::getPinState(byte pin)
+{
+  return pinState[pin];
+}
+
+/**
+ * Set the pin state. The pin state of an output pin is the pin value. The state of an
+ * input pin is 0, unless the pin has it's internal pull up resistor enabled, then the value is 1.
+ * @param pin The pin to set the state of
+ * @param state Set the state of the specified pin
+ */
+void FirmataClass::setPinState(byte pin, int state)
+{
+  pinState[pin] = state;
+}
+
+// sysex callbacks
+/*
+ * this is too complicated for analogReceive, but maybe for Sysex?
+ void FirmataClass::attachSysex(sysexFunction newFunction)
+ {
+ byte i;
+ byte tmpCount = analogReceiveFunctionCount;
+ analogReceiveFunction* tmpArray = analogReceiveFunctionArray;
+ analogReceiveFunctionCount++;
+ analogReceiveFunctionArray = (analogReceiveFunction*) calloc(analogReceiveFunctionCount, sizeof(analogReceiveFunction));
+ for(i = 0; i < tmpCount; i++) {
+ analogReceiveFunctionArray[i] = tmpArray[i];
+ }
+ analogReceiveFunctionArray[tmpCount] = newFunction;
+ free(tmpArray);
+ }
+*/
+
+//******************************************************************************
+//* Private Methods
+//******************************************************************************
+
+/**
+ * Resets the system state upon a SYSTEM_RESET message from the host software.
+ * @private
+ */
+void FirmataClass::systemReset(void)
+{
+  byte i;
+
+  waitForData = 0; // this flag says the next serial input will be data
+  executeMultiByteCommand = 0; // execute this after getting multi-byte data
+  multiByteChannel = 0; // channel data for multiByteCommands
+
+  for (i = 0; i < MAX_DATA_BYTES; i++) {
+    storedInputData[i] = 0;
+  }
+
+  parsingSysex = false;
+  sysexBytesRead = 0;
+
+  if (currentSystemResetCallback)
+    (*currentSystemResetCallback)();
+}
+
+/**
+ * Flashing the pin for the version number
+ * @private
+ * @param pin The pin the LED is attached to.
+ * @param count The number of times to flash the LED.
+ * @param onInterval The number of milliseconds for the LED to be ON during each interval.
+ * @param offInterval The number of milliseconds for the LED to be OFF during each interval.
+ */
+void FirmataClass::strobeBlinkPin(byte pin, int count, int onInterval, int offInterval)
+{
+  byte i;
+  for (i = 0; i < count; i++) {
+    delay(offInterval);
+    digitalWrite(pin, HIGH);
+    delay(onInterval);
+    digitalWrite(pin, LOW);
+  }
+}
+
+// make one instance for the user to use
+FirmataClass Firmata;
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/Firmata.h	(revision 224)
@@ -0,0 +1,224 @@
+/*
+  Firmata.h - Firmata library v2.5.2 - 2016-2-15
+  Copyright (c) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2009-2015 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef Firmata_h
+#define Firmata_h
+
+#include "Boards.h"  /* Hardware Abstraction Layer + Wiring/Arduino */
+
+/* Version numbers for the protocol.  The protocol is still changing, so these
+ * version numbers are important.
+ * Query using the REPORT_VERSION message.
+ */
+#define FIRMATA_PROTOCOL_MAJOR_VERSION  2 // for non-compatible changes
+#define FIRMATA_PROTOCOL_MINOR_VERSION  5 // for backwards compatible changes
+#define FIRMATA_PROTOCOL_BUGFIX_VERSION 1 // for bugfix releases
+
+/* Version numbers for the Firmata library.
+ * The firmware version will not always equal the protocol version going forward.
+ * Query using the REPORT_FIRMWARE message.
+ */
+#define FIRMATA_FIRMWARE_MAJOR_VERSION  2
+#define FIRMATA_FIRMWARE_MINOR_VERSION  5
+#define FIRMATA_FIRMWARE_BUGFIX_VERSION 2
+
+/* DEPRECATED as of Firmata v2.5.1. As of 2.5.1 there are separate version numbers for
+ * the protocol version and the firmware version.
+ */
+#define FIRMATA_MAJOR_VERSION           2 // same as FIRMATA_PROTOCOL_MAJOR_VERSION
+#define FIRMATA_MINOR_VERSION           5 // same as FIRMATA_PROTOCOL_MINOR_VERSION
+#define FIRMATA_BUGFIX_VERSION          1 // same as FIRMATA_PROTOCOL_BUGFIX_VERSION
+
+#define MAX_DATA_BYTES                  64 // max number of data bytes in incoming messages
+
+// Arduino 101 also defines SET_PIN_MODE as a macro in scss_registers.h
+#ifdef SET_PIN_MODE
+#undef SET_PIN_MODE
+#endif
+
+// message command bytes (128-255/0x80-0xFF)
+#define DIGITAL_MESSAGE         0x90 // send data for a digital port (collection of 8 pins)
+#define ANALOG_MESSAGE          0xE0 // send data for an analog pin (or PWM)
+#define REPORT_ANALOG           0xC0 // enable analog input by pin #
+#define REPORT_DIGITAL          0xD0 // enable digital input by port pair
+//
+#define SET_PIN_MODE            0xF4 // set a pin to INPUT/OUTPUT/PWM/etc
+#define SET_DIGITAL_PIN_VALUE   0xF5 // set value of an individual digital pin
+//
+#define REPORT_VERSION          0xF9 // report protocol version
+#define SYSTEM_RESET            0xFF // reset from MIDI
+//
+#define START_SYSEX             0xF0 // start a MIDI Sysex message
+#define END_SYSEX               0xF7 // end a MIDI Sysex message
+
+// extended command set using sysex (0-127/0x00-0x7F)
+/* 0x00-0x0F reserved for user-defined commands */
+#define SERIAL_MESSAGE          0x60 // communicate with serial devices, including other boards
+#define ENCODER_DATA            0x61 // reply with encoders current positions
+#define SERVO_CONFIG            0x70 // set max angle, minPulse, maxPulse, freq
+#define STRING_DATA             0x71 // a string message with 14-bits per char
+#define STEPPER_DATA            0x72 // control a stepper motor
+#define ONEWIRE_DATA            0x73 // send an OneWire read/write/reset/select/skip/search request
+#define SHIFT_DATA              0x75 // a bitstream to/from a shift register
+#define I2C_REQUEST             0x76 // send an I2C read/write request
+#define I2C_REPLY               0x77 // a reply to an I2C read request
+#define I2C_CONFIG              0x78 // config I2C settings such as delay times and power pins
+#define EXTENDED_ANALOG         0x6F // analog write (PWM, Servo, etc) to any pin
+#define PIN_STATE_QUERY         0x6D // ask for a pin's current mode and value
+#define PIN_STATE_RESPONSE      0x6E // reply with pin's current mode and value
+#define CAPABILITY_QUERY        0x6B // ask for supported modes and resolution of all pins
+#define CAPABILITY_RESPONSE     0x6C // reply with supported modes and resolution
+#define ANALOG_MAPPING_QUERY    0x69 // ask for mapping of analog to pin numbers
+#define ANALOG_MAPPING_RESPONSE 0x6A // reply with mapping info
+#define REPORT_FIRMWARE         0x79 // report name and version of the firmware
+#define SAMPLING_INTERVAL       0x7A // set the poll rate of the main loop
+#define SCHEDULER_DATA          0x7B // send a createtask/deletetask/addtotask/schedule/querytasks/querytask request to the scheduler
+#define SYSEX_NON_REALTIME      0x7E // MIDI Reserved for non-realtime messages
+#define SYSEX_REALTIME          0x7F // MIDI Reserved for realtime messages
+// these are DEPRECATED to make the naming more consistent
+#define FIRMATA_STRING          0x71 // same as STRING_DATA
+#define SYSEX_I2C_REQUEST       0x76 // same as I2C_REQUEST
+#define SYSEX_I2C_REPLY         0x77 // same as I2C_REPLY
+#define SYSEX_SAMPLING_INTERVAL 0x7A // same as SAMPLING_INTERVAL
+
+// pin modes
+//#define INPUT                 0x00 // defined in Arduino.h
+//#define OUTPUT                0x01 // defined in Arduino.h
+#define PIN_MODE_ANALOG         0x02 // analog pin in analogInput mode
+#define PIN_MODE_PWM            0x03 // digital pin in PWM output mode
+#define PIN_MODE_SERVO          0x04 // digital pin in Servo output mode
+#define PIN_MODE_SHIFT          0x05 // shiftIn/shiftOut mode
+#define PIN_MODE_I2C            0x06 // pin included in I2C setup
+#define PIN_MODE_ONEWIRE        0x07 // pin configured for 1-wire
+#define PIN_MODE_STEPPER        0x08 // pin configured for stepper motor
+#define PIN_MODE_ENCODER        0x09 // pin configured for rotary encoders
+#define PIN_MODE_SERIAL         0x0A // pin configured for serial communication
+#define PIN_MODE_PULLUP         0x0B // enable internal pull-up resistor for pin
+#define PIN_MODE_IGNORE         0x7F // pin configured to be ignored by digitalWrite and capabilityResponse
+#define TOTAL_PIN_MODES         13
+// DEPRECATED as of Firmata v2.5
+#define ANALOG                  0x02 // same as PIN_MODE_ANALOG
+#define PWM                     0x03 // same as PIN_MODE_PWM
+#define SERVO                   0x04 // same as PIN_MODE_SERVO
+#define SHIFT                   0x05 // same as PIN_MODE_SHIFT
+#define I2C                     0x06 // same as PIN_MODE_I2C
+#define ONEWIRE                 0x07 // same as PIN_MODE_ONEWIRE
+#define STEPPER                 0x08 // same as PIN_MODE_STEPPER
+#define ENCODER                 0x09 // same as PIN_MODE_ENCODER
+#define IGNORE                  0x7F // same as PIN_MODE_IGNORE
+
+extern "C" {
+  // callback function types
+  typedef void (*callbackFunction)(byte, int);
+  typedef void (*systemResetCallbackFunction)(void);
+  typedef void (*stringCallbackFunction)(char *);
+  typedef void (*sysexCallbackFunction)(byte command, byte argc, byte *argv);
+}
+
+// TODO make it a subclass of a generic Serial/Stream base class
+class FirmataClass
+{
+  public:
+    FirmataClass();
+    /* Arduino constructors */
+    void begin();
+    void begin(long);
+    void begin(Stream &s);
+    /* querying functions */
+    void printVersion(void);
+    void blinkVersion(void);
+    void printFirmwareVersion(void);
+    //void setFirmwareVersion(byte major, byte minor);  // see macro below
+    void setFirmwareNameAndVersion(const char *name, byte major, byte minor);
+    void disableBlinkVersion();
+    /* serial receive handling */
+    int available(void);
+    void processInput(void);
+    void parse(unsigned char value);
+    boolean isParsingMessage(void);
+    /* serial send handling */
+    void sendAnalog(byte pin, int value);
+    void sendDigital(byte pin, int value); // TODO implement this
+    void sendDigitalPort(byte portNumber, int portData);
+    void sendString(const char *string);
+    void sendString(byte command, const char *string);
+    void sendSysex(byte command, byte bytec, byte *bytev);
+    void write(byte c);
+    /* attach & detach callback functions to messages */
+    void attach(byte command, callbackFunction newFunction);
+    void attach(byte command, systemResetCallbackFunction newFunction);
+    void attach(byte command, stringCallbackFunction newFunction);
+    void attach(byte command, sysexCallbackFunction newFunction);
+    void detach(byte command);
+
+    /* access pin state and config */
+    byte getPinMode(byte pin);
+    void setPinMode(byte pin, byte config);
+    /* access pin state */
+    int getPinState(byte pin);
+    void setPinState(byte pin, int state);
+
+    /* utility methods */
+    void sendValueAsTwo7bitBytes(int value);
+    void startSysex(void);
+    void endSysex(void);
+
+  private:
+    Stream *FirmataStream;
+    /* firmware name and version */
+    byte firmwareVersionCount;
+    byte *firmwareVersionVector;
+    /* input message handling */
+    byte waitForData; // this flag says the next serial input will be data
+    byte executeMultiByteCommand; // execute this after getting multi-byte data
+    byte multiByteChannel; // channel data for multiByteCommands
+    byte storedInputData[MAX_DATA_BYTES]; // multi-byte data
+    /* sysex */
+    boolean parsingSysex;
+    int sysexBytesRead;
+    /* pin configuration */
+    byte pinConfig[TOTAL_PINS];
+    int pinState[TOTAL_PINS];
+
+    /* callback functions */
+    callbackFunction currentAnalogCallback;
+    callbackFunction currentDigitalCallback;
+    callbackFunction currentReportAnalogCallback;
+    callbackFunction currentReportDigitalCallback;
+    callbackFunction currentPinModeCallback;
+    callbackFunction currentPinValueCallback;
+    systemResetCallbackFunction currentSystemResetCallback;
+    stringCallbackFunction currentStringCallback;
+    sysexCallbackFunction currentSysexCallback;
+
+    boolean blinkVersionDisabled = false;
+
+    /* private methods ------------------------------ */
+    void processSysexMessage(void);
+    void systemReset(void);
+    void strobeBlinkPin(byte pin, int count, int onInterval, int offInterval);
+};
+
+extern FirmataClass Firmata;
+
+/*==============================================================================
+ * MACROS
+ *============================================================================*/
+
+/* shortcut for setFirmwareNameAndVersion() that uses __FILE__ to set the
+ * firmware name.  It needs to be a macro so that __FILE__ is included in the
+ * firmware source file rather than the library source file.
+ */
+#define setFirmwareVersion(x, y)   setFirmwareNameAndVersion(__FILE__, x, y)
+
+#endif /* Firmata_h */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AllInputsFirmata/AllInputsFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AllInputsFirmata/AllInputsFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AllInputsFirmata/AllInputsFirmata.ino	(revision 224)
@@ -23,5 +23,5 @@
 int previousAnalogValues[TOTAL_ANALOG_PINS];
 
-byte portStatus[TOTAL_PORTS];	// each bit: 1=pin is digital input, 0=other/ignore
+byte portStatus[TOTAL_PORTS]; // each bit: 1=pin is digital input, 0=other/ignore
 byte previousPINs[TOTAL_PORTS];
 
@@ -46,5 +46,5 @@
   byte i, port, status;
 
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
 
   for (pin = 0; pin < TOTAL_PINS; pin++) {
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/AnalogFirmata/AnalogFirmata.ino	(revision 224)
@@ -54,5 +54,5 @@
 {
   if (value == 0) {
-    analogInputsToReport = analogInputsToReport &~ (1 << pin);
+    analogInputsToReport = analogInputsToReport & ~ (1 << pin);
   }
   else { // everything but 0 enables reporting of that pin
@@ -67,5 +67,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 2);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
   Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/EchoString/EchoString.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/EchoString/EchoString.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/EchoString/EchoString.ino	(revision 224)
@@ -22,5 +22,5 @@
 
 
-void sysexCallback(byte command, byte argc, byte*argv)
+void sysexCallback(byte command, byte argc, byte *argv)
 {
   Firmata.sendSysex(command, argc, argv);
@@ -29,5 +29,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(STRING_DATA, stringCallback);
   Firmata.attach(START_SYSEX, sysexCallback);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/OldStandardFirmata/OldStandardFirmata.ino	(revision 224)
@@ -56,5 +56,5 @@
 void outputPort(byte portNumber, byte portValue)
 {
-  portValue = portValue &~ portStatus[portNumber];
+  portValue = portValue & ~ portStatus[portNumber];
   if (previousPINs[portNumber] != portValue) {
     Firmata.sendDigitalPort(portNumber, portValue);
@@ -73,5 +73,5 @@
     if (reportPINs[i]) {
       switch (i) {
-        case 0: outputPort(0, PIND &~ B00000011); break; // ignore Rx/Tx 0/1
+        case 0: outputPort(0, PIND & ~ B00000011); break; // ignore Rx/Tx 0/1
         case 1: outputPort(1, PINB); break;
         case 2: outputPort(2, PINC); break;
@@ -105,5 +105,5 @@
       case INPUT:
         pinMode(pin, INPUT);
-        portStatus[port] = portStatus[port] &~ (1 << (pin - offset));
+        portStatus[port] = portStatus[port] & ~ (1 << (pin - offset));
         break;
       case OUTPUT:
@@ -113,5 +113,5 @@
         portStatus[port] = portStatus[port] | (1 << (pin - offset));
         break;
-        //case ANALOG: // TODO figure this out
+      //case ANALOG: // TODO figure this out
       default:
         Firmata.sendString("");
@@ -123,5 +123,5 @@
 void analogWriteCallback(byte pin, int value)
 {
-  setPinModeCallback(pin, PWM);
+  setPinModeCallback(pin, PIN_MODE_PWM);
   analogWrite(pin, value);
 }
@@ -132,5 +132,5 @@
     case 0: // pins 2-7 (don't change Rx/Tx, pins 0 and 1)
       // 0xFF03 == B1111111100000011    0x03 == B00000011
-      PORTD = (value &~ 0xFF03) | (PORTD & 0x03);
+      PORTD = (value & ~ 0xFF03) | (PORTD & 0x03);
       break;
     case 1: // pins 8-13 (14,15 are disabled for the crystal)
@@ -151,5 +151,5 @@
 {
   if (value == 0) {
-    analogInputsToReport = analogInputsToReport &~ (1 << pin);
+    analogInputsToReport = analogInputsToReport & ~ (1 << pin);
   }
   else { // everything but 0 enables reporting of that pin
@@ -203,5 +203,5 @@
    * host computer, since once in the loop(), this firmware will only send
    * digital data on change. */
-  if (reportPINs[0]) outputPort(0, PIND &~ B00000011); // ignore Rx/Tx 0/1
+  if (reportPINs[0]) outputPort(0, PIND & ~ B00000011); // ignore Rx/Tx 0/1
   if (reportPINs[1]) outputPort(1, PINB);
   if (reportPINs[2]) outputPort(2, PINC);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/ServoFirmata/ServoFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/ServoFirmata/ServoFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/ServoFirmata/ServoFirmata.ino	(revision 224)
@@ -13,6 +13,4 @@
  * included in Arduino 0017
  *
- * TODO add message to configure minPulse/maxPulse/degrees
- *
  * This example code is in the public domain.
  */
@@ -22,10 +20,17 @@
 
 Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte servoCount = 0;
 
 void analogWriteCallback(byte pin, int value)
 {
-  if (IS_PIN_SERVO(pin)) {
-    servos[PIN_TO_SERVO(pin)].write(value);
+  if (IS_PIN_DIGITAL(pin)) {
+    servos[servoPinMap[pin]].write(value);
   }
+}
+
+void systemResetCallback()
+{
+  servoCount = 0;
 }
 
@@ -34,14 +39,22 @@
   byte pin;
 
-  Firmata.setFirmwareVersion(0, 2);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
 
+  Firmata.begin(57600);
+  systemResetCallback();
+
+  // attach servos from first digital pin up to max number of
+  // servos supported for the board
   for (pin = 0; pin < TOTAL_PINS; pin++) {
-    if (IS_PIN_SERVO(pin)) {
-      servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
+    if (IS_PIN_DIGITAL(pin)) {
+      if (servoCount < MAX_SERVOS) {
+        servoPinMap[pin] = servoCount;
+        servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+        servoCount++;
+      }
     }
   }
-
-  Firmata.begin(57600);
 }
 
@@ -51,3 +64,2 @@
     Firmata.processInput();
 }
-
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleAnalogFirmata/SimpleAnalogFirmata.ino	(revision 224)
@@ -28,5 +28,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
   Firmata.begin(57600);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/SimpleDigitalFirmata/SimpleDigitalFirmata.ino	(revision 224)
@@ -53,5 +53,5 @@
 void setup()
 {
-  Firmata.setFirmwareVersion(0, 1);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
   Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
   Firmata.attach(SET_PIN_MODE, setPinModeCallback);
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmata/StandardFirmata.ino	(revision 224)
@@ -1,18 +1,16 @@
 /*
- * Firmata is a generic protocol for communicating with microcontrollers
- * from software on a host computer. It is intended to work with
- * any host computer software package.
- *
- * To download a host software package, please clink on the following link
- * to open the download page in your default browser.
- *
- * http://firmata.org/wiki/Download
- */
-
-/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
   Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
   Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
   Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
-  Copyright (C) 2009-2011 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
 
   This library is free software; you can redistribute it and/or
@@ -23,10 +21,6 @@
   See file LICENSE.txt for further informations on licensing terms.
 
-  formatted using the GNU C formatting and indenting
+  Last updated by Jeff Hoefs: January 10th, 2016
 */
-
-/*
- * TODO: use Program Control to load stored profiles from EEPROM
- */
 
 #include <Servo.h>
@@ -34,16 +28,19 @@
 #include <Firmata.h>
 
-// move the following defines to Firmata.h?
-#define I2C_WRITE B00000000
-#define I2C_READ B00001000
-#define I2C_READ_CONTINUOUSLY B00010000
-#define I2C_STOP_READING B00011000
-#define I2C_READ_WRITE_MODE_MASK B00011000
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
 #define I2C_10BIT_ADDRESS_MODE_MASK B00100000
-
-#define MAX_QUERIES 8
-#define MINIMUM_SAMPLING_INTERVAL 10
-
-#define REGISTER_NOT_SPECIFIED -1
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
 
 /*==============================================================================
@@ -51,4 +48,8 @@
  *============================================================================*/
 
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
 /* analog inputs */
 int analogInputsToReport = 0; // bitwise array to store pin reporting
@@ -59,45 +60,106 @@
 
 /* pins configuration */
-byte pinConfig[TOTAL_PINS];         // configuration of every pin
 byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
-int pinState[TOTAL_PINS];           // any value that has been written
 
 /* timer variables */
 unsigned long currentMillis;        // store the current value from millis()
 unsigned long previousMillis;       // for comparison with currentMillis
-int samplingInterval = 19;          // how often to run the main loop (in ms)
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
 
 /* i2c data */
 struct i2c_device_info {
   byte addr;
-  byte reg;
+  int reg;
   byte bytes;
+  byte stopTX;
 };
 
 /* for i2c read continuous more */
-i2c_device_info query[MAX_QUERIES];
-
-byte i2cRxData[32];
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
 boolean isI2CEnabled = false;
 signed char queryIndex = -1;
-unsigned int i2cReadDelayTime = 0;  // default delay time between i2c read request and Wire.requestFrom()
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
 
 Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
 /*==============================================================================
  * FUNCTIONS
  *============================================================================*/
 
-void readAndReportData(byte address, int theRegister, byte numBytes) {
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
   // allow I2C requests that don't require a register read
   // for example, some devices using an interrupt pin to signify new data available
   // do not always require the register read so upon interrupt you call Wire.requestFrom()
-  if (theRegister != REGISTER_NOT_SPECIFIED) {
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
     Wire.beginTransmission(address);
-#if ARDUINO >= 100
-    Wire.write((byte)theRegister);
-#else
-    Wire.send((byte)theRegister);
-#endif
-    Wire.endTransmission();
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
     // do not set a value of 0
     if (i2cReadDelayTime > 0) {
@@ -112,21 +174,15 @@
 
   // check to be sure correct number of bytes were returned by slave
-  if (numBytes == Wire.available()) {
-    i2cRxData[0] = address;
-    i2cRxData[1] = theRegister;
-    for (int i = 0; i < numBytes; i++) {
-#if ARDUINO >= 100
-      i2cRxData[2 + i] = Wire.read();
-#else
-      i2cRxData[2 + i] = Wire.receive();
-#endif
-    }
-  }
-  else {
-    if (numBytes > Wire.available()) {
-      Firmata.sendString("I2C Read Error: Too many bytes received");
-    } else {
-      Firmata.sendString("I2C Read Error: Too few bytes received");
-    }
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
   }
 
@@ -173,4 +229,37 @@
 
 // -----------------------------------------------------------------------------
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+// -----------------------------------------------------------------------------
 /* sets the pin mode to the correct state and sets the relevant bits in the
  * two bit-arrays that track Digital I/O and PWM status
@@ -178,17 +267,22 @@
 void setPinModeCallback(byte pin, int mode)
 {
-  if (pinConfig[pin] == I2C && isI2CEnabled && mode != I2C) {
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
     // disable i2c so pins can be used for other functions
     // the following if statements should reconfigure the pins properly
     disableI2CPins();
   }
-  if (IS_PIN_SERVO(pin) && mode != SERVO && servos[PIN_TO_SERVO(pin)].attached()) {
-    servos[PIN_TO_SERVO(pin)].detach();
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
   }
   if (IS_PIN_ANALOG(pin)) {
-    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == ANALOG ? 1 : 0); // turn on/off reporting
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
   }
   if (IS_PIN_DIGITAL(pin)) {
-    if (mode == INPUT) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
       portConfigInputs[pin / 8] |= (1 << (pin & 7));
     } else {
@@ -196,20 +290,33 @@
     }
   }
-  pinState[pin] = 0;
+  Firmata.setPinState(pin, 0);
   switch (mode) {
-    case ANALOG:
+    case PIN_MODE_ANALOG:
       if (IS_PIN_ANALOG(pin)) {
         if (IS_PIN_DIGITAL(pin)) {
-          pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
           digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
-        }
-        pinConfig[pin] = ANALOG;
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
       }
       break;
     case INPUT:
       if (IS_PIN_DIGITAL(pin)) {
-        pinMode(PIN_TO_DIGITAL(pin), INPUT); // disable output driver
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
-        pinConfig[pin] = INPUT;
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
       }
       break;
@@ -218,28 +325,35 @@
         digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
         pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
-        pinConfig[pin] = OUTPUT;
-      }
-      break;
-    case PWM:
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
       if (IS_PIN_PWM(pin)) {
         pinMode(PIN_TO_PWM(pin), OUTPUT);
         analogWrite(PIN_TO_PWM(pin), 0);
-        pinConfig[pin] = PWM;
-      }
-      break;
-    case SERVO:
-      if (IS_PIN_SERVO(pin)) {
-        pinConfig[pin] = SERVO;
-        if (!servos[PIN_TO_SERVO(pin)].attached()) {
-          servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin));
-        }
-      }
-      break;
-    case I2C:
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
       if (IS_PIN_I2C(pin)) {
         // mark the pin as i2c
         // the user must call I2C_CONFIG to enable I2C for a device
-        pinConfig[pin] = I2C;
-      }
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
       break;
     default:
@@ -249,17 +363,33 @@
 }
 
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
 void analogWriteCallback(byte pin, int value)
 {
   if (pin < TOTAL_PINS) {
-    switch (pinConfig[pin]) {
-      case SERVO:
-        if (IS_PIN_SERVO(pin))
-          servos[PIN_TO_SERVO(pin)].write(value);
-        pinState[pin] = value;
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
         break;
-      case PWM:
+      case PIN_MODE_PWM:
         if (IS_PIN_PWM(pin))
           analogWrite(PIN_TO_PWM(pin), value);
-        pinState[pin] = value;
+        Firmata.setPinState(pin, value);
         break;
     }
@@ -269,5 +399,5 @@
 void digitalWriteCallback(byte port, int value)
 {
-  byte pin, lastPin, mask = 1, pinWriteMask = 0;
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
 
   if (port < TOTAL_PORTS) {
@@ -278,9 +408,19 @@
       // do not disturb non-digital pins (eg, Rx & Tx)
       if (IS_PIN_DIGITAL(pin)) {
-        // only write to OUTPUT and INPUT (enables pullup)
         // do not touch pins in PWM, ANALOG, SERVO or other modes
-        if (pinConfig[pin] == OUTPUT || pinConfig[pin] == INPUT) {
-          pinWriteMask |= mask;
-          pinState[pin] = ((byte)value & mask) ? 1 : 0;
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
         }
       }
@@ -293,19 +433,4 @@
 
 // -----------------------------------------------------------------------------
-/* sets bits in a bit array (int) to toggle the reporting of the analogIns
- */
-//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
-//}
-void reportAnalogCallback(byte analogPin, int value)
-{
-  if (analogPin < TOTAL_ANALOG_PINS) {
-    if (value == 0) {
-      analogInputsToReport = analogInputsToReport &~ (1 << analogPin);
-    } else {
-      analogInputsToReport = analogInputsToReport | (1 << analogPin);
-    }
-  }
-  // TODO: save status to EEPROM here, if changed
-}
 
 void reportDigitalCallback(byte port, int value)
@@ -313,4 +438,8 @@
   if (port < TOTAL_PORTS) {
     reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
   }
   // do not disable analog reporting on these 8 pins, to allow some
@@ -322,4 +451,23 @@
 }
 
+// -----------------------------------------------------------------------------
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
 /*==============================================================================
  * SYSEX-BASED commands
@@ -329,7 +477,8 @@
 {
   byte mode;
+  byte stopTX;
   byte slaveAddress;
-  byte slaveRegister;
   byte data;
+  int slaveRegister;
   unsigned int delayTime;
 
@@ -338,9 +487,18 @@
       mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
       if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
-        Firmata.sendString("10-bit addressing mode is not yet supported");
+        Firmata.sendString("10-bit addressing not supported");
         return;
       }
       else {
         slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
       }
 
@@ -350,9 +508,5 @@
           for (byte i = 2; i < argc; i += 2) {
             data = argv[i] + (argv[i + 1] << 7);
-#if ARDUINO >= 100
-            Wire.write(data);
-#else
-            Wire.send(data);
-#endif
+            wireWrite(data);
           }
           Wire.endTransmission();
@@ -364,22 +518,33 @@
             slaveRegister = argv[2] + (argv[3] << 7);
             data = argv[4] + (argv[5] << 7);  // bytes to read
-            readAndReportData(slaveAddress, (int)slaveRegister, data);
           }
           else {
             // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
             data = argv[2] + (argv[3] << 7);  // bytes to read
-            readAndReportData(slaveAddress, (int)REGISTER_NOT_SPECIFIED, data);
           }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
           break;
         case I2C_READ_CONTINUOUSLY:
-          if ((queryIndex + 1) >= MAX_QUERIES) {
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
             // too many queries, just ignore
             Firmata.sendString("too many queries");
             break;
           }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
           queryIndex++;
           query[queryIndex].addr = slaveAddress;
-          query[queryIndex].reg = argv[2] + (argv[3] << 7);
-          query[queryIndex].bytes = argv[4] + (argv[5] << 7);
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
           break;
         case I2C_STOP_READING:
@@ -390,9 +555,10 @@
             queryIndex = -1;
           } else {
+            queryIndexToSkip = 0;
             // if read continuous mode is enabled for multiple devices,
             // determine which device to stop reading and remove it's data from
             // the array, shifiting other array data to fill the space
             for (byte i = 0; i < queryIndex + 1; i++) {
-              if (query[i].addr = slaveAddress) {
+              if (query[i].addr == slaveAddress) {
                 queryIndexToSkip = i;
                 break;
@@ -401,8 +567,9 @@
 
             for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
-              if (i < MAX_QUERIES) {
+              if (i < I2C_MAX_QUERIES) {
                 query[i].addr = query[i + 1].addr;
-                query[i].reg = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
                 query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
               }
             }
@@ -433,9 +600,10 @@
         int maxPulse = argv[3] + (argv[4] << 7);
 
-        if (IS_PIN_SERVO(pin)) {
-          if (servos[PIN_TO_SERVO(pin)].attached())
-            servos[PIN_TO_SERVO(pin)].detach();
-          servos[PIN_TO_SERVO(pin)].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
-          setPinModeCallback(pin, SERVO);
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
         }
       }
@@ -466,23 +634,28 @@
           Firmata.write((byte)INPUT);
           Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
           Firmata.write((byte)OUTPUT);
           Firmata.write(1);
         }
         if (IS_PIN_ANALOG(pin)) {
-          Firmata.write(ANALOG);
-          Firmata.write(10);
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
         }
         if (IS_PIN_PWM(pin)) {
-          Firmata.write(PWM);
-          Firmata.write(8);
-        }
-        if (IS_PIN_SERVO(pin)) {
-          Firmata.write(SERVO);
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
           Firmata.write(14);
         }
         if (IS_PIN_I2C(pin)) {
-          Firmata.write(I2C);
-          Firmata.write(1);  // to do: determine appropriate value
-        }
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
         Firmata.write(127);
       }
@@ -496,8 +669,8 @@
         Firmata.write(pin);
         if (pin < TOTAL_PINS) {
-          Firmata.write((byte)pinConfig[pin]);
-          Firmata.write((byte)pinState[pin] & 0x7F);
-          if (pinState[pin] & 0xFF80) Firmata.write((byte)(pinState[pin] >> 7) & 0x7F);
-          if (pinState[pin] & 0xC000) Firmata.write((byte)(pinState[pin] >> 14) & 0x7F);
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
         }
         Firmata.write(END_SYSEX);
@@ -512,33 +685,13 @@
       Firmata.write(END_SYSEX);
       break;
-  }
-}
-
-void enableI2CPins()
-{
-  byte i;
-  // is there a faster way to do this? would probaby require importing
-  // Arduino.h to get SCL and SDA pins
-  for (i = 0; i < TOTAL_PINS; i++) {
-    if (IS_PIN_I2C(i)) {
-      // mark pins as i2c so they are ignore in non i2c data requests
-      setPinModeCallback(i, I2C);
-    }
-  }
-
-  isI2CEnabled = true;
-
-  // is there enough time before the first I2C request to call this here?
-  Wire.begin();
-}
-
-/* disable the i2c pins so they can be used for other functions */
-void disableI2CPins() {
-  isI2CEnabled = false;
-  // disable read continuous mode for all devices
-  queryIndex = -1;
-  // uncomment the following if or when the end() method is added to Wire library
-  // Wire.end();
-}
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
 
 /*==============================================================================
@@ -548,27 +701,41 @@
 void systemResetCallback()
 {
+  isResetting = true;
+
   // initialize a defalt state
   // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
   if (isI2CEnabled) {
     disableI2CPins();
   }
+
   for (byte i = 0; i < TOTAL_PORTS; i++) {
-    reportPINs[i] = false;      // by default, reporting off
-    portConfigInputs[i] = 0;	// until activated
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
     previousPINs[i] = 0;
   }
-  // pins with analog capability default to analog input
-  // otherwise, pins default to digital output
+
   for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
     if (IS_PIN_ANALOG(i)) {
       // turns off pullup, configures everything
-      setPinModeCallback(i, ANALOG);
-    } else {
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
       // sets the output to 0, configures portConfigInputs
       setPinModeCallback(i, OUTPUT);
     }
+
+    servoPinMap[i] = 255;
   }
   // by default, do not report any analog inputs
   analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
 
   /* send digital inputs to set the initial state on the host computer,
@@ -581,9 +748,10 @@
   }
   */
+  isResetting = false;
 }
 
 void setup()
 {
-  Firmata.setFirmwareVersion(FIRMATA_MAJOR_VERSION, FIRMATA_MINOR_VERSION);
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
 
   Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
@@ -592,8 +760,19 @@
   Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
   Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
   Firmata.attach(START_SYSEX, sysexCallback);
   Firmata.attach(SYSTEM_RESET, systemResetCallback);
 
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
   Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
   systemResetCallback();  // reset to default config
 }
@@ -610,12 +789,10 @@
   checkDigitalInputs();
 
-  /* SERIALREAD - processing incoming messagse as soon as possible, while still
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
    * checking digital inputs.  */
   while (Firmata.available())
     Firmata.processInput();
 
-  /* SEND FTDI WRITE BUFFER - make sure that the FTDI buffer doesn't go over
-   * 60 bytes. use a timer to sending an event character every 4 ms to
-   * trigger the buffer to dump. */
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
 
   currentMillis = millis();
@@ -624,5 +801,5 @@
     /* ANALOGREAD - do all analogReads() at the configured sampling interval */
     for (pin = 0; pin < TOTAL_PINS; pin++) {
-      if (IS_PIN_ANALOG(pin) && pinConfig[pin] == ANALOG) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
         analogPin = PIN_TO_ANALOG(pin);
         if (analogInputsToReport & (1 << analogPin)) {
@@ -634,7 +811,11 @@
     if (queryIndex > -1) {
       for (byte i = 0; i < queryIndex + 1; i++) {
-        readAndReportData(query[i].addr, query[i].reg, query[i].bytes);
-      }
-    }
-  }
-}
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/StandardFirmataBLE.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/StandardFirmataBLE.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/StandardFirmataBLE.ino	(revision 224)
@@ -0,0 +1,850 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated March 13th, 2016
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+/*
+ * Uncomment the following include to enable interfacing
+ * with Serial devices via hardware or software serial.
+ */
+//#include "utility/SerialFirmata.h"
+
+// follow the instructions in bleConfig.h to configure your BLE hardware
+#include "bleConfig.h"
+
+#define I2C_WRITE                   0x00 //B00000000
+#define I2C_READ                    0x08 //B00001000
+#define I2C_READ_CONTINUOUSLY       0x10 //B00010000
+#define I2C_STOP_READING            0x18 //B00011000
+#define I2C_READ_WRITE_MODE_MASK    0x18 //B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK 0x20 //B00100000
+#define I2C_END_TX_MASK             0x40 //B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+// min cannot be < 0x0006. Adjust max if necessary
+#define FIRMATA_BLE_MIN_INTERVAL    0x0006 // 7.5ms (7.5 / 1.25)
+#define FIRMATA_BLE_MAX_INTERVAL    0x0018 // 30ms (30 / 1.25)
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+// -----------------------------------------------------------------------------
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  stream.setLocalName(FIRMATA_BLE_LOCAL_NAME);
+
+// setConnectionInterval is not available in the CurieBLE library included in the
+// Intel Curie Boards package v1.0.5 (latest version via the Boards Manager). Without
+// setConnectionInterval, the BLE reporting rate for analog input and I2C read continuous mode
+// will be very slow (150ms instead of 30ms).
+// However, you can manually update CurieBLE to get the functionality now. Follow these steps:
+// 1. Install Intel Curie Boards v1.0.5 via the Arduino Boards manager (use Arduino 1.6.7 or newer)
+// 2. Download or clone corelibs-arduino101: https://github.com/01org/corelibs-arduino101
+// 3. Make a copy of the CurieBLE directory found in corelibs-arduino101/libraries/
+// 4. Find the Arduino15 directory on your computer:
+//    OS X:    ~/Library/Arduino15
+//    Windows: C:\Users\(username)\AppData\Local\Arduino15
+//    Linux:   ~/.arduino15
+// 5. From the Arduino15 directory, navigate to: /packages/Intel/hardware/arc32/1.0.5/libraries/
+// 6. Replace the CurieBLE library with the version you copied in step 3
+// 7. Comment out the #ifndef statement below and the following #endif statement
+#ifndef _VARIANT_ARDUINO_101_X_
+  // set the BLE connection interval - this is the fastest interval you can read inputs
+  stream.setConnectionInterval(FIRMATA_BLE_MIN_INTERVAL, FIRMATA_BLE_MAX_INTERVAL);
+  // set how often the BLE TX buffer is flushed (if not full)
+  stream.setFlushInterval(FIRMATA_BLE_MAX_INTERVAL);
+#endif
+
+#ifdef BLE_REQ
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_IGNORE_BLE_PINS(i)) {
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+#endif
+
+  stream.begin();
+  Firmata.begin(stream);
+
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  // do not process data if no BLE connection is established
+  // poll will send the TX buffer at the specified flush interval or when the buffer is full
+  if (!stream.poll()) return;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis = currentMillis;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/bleConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/bleConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataBLE/bleConfig.h	(revision 224)
@@ -0,0 +1,106 @@
+/*==================================================================================================
+ * BLE CONFIGURATION
+ *
+ * If you are using an Arduino 101, you do not need to make any changes to this file (unless you
+ * need a unique ble local name (see below). If you are using another supported BLE board or shield,
+ * follow the instructions for the specific board or shield below.
+ *
+ * Supported boards and shields:
+ * - Arduino 101 (recommended)
+ * - RedBearLab BLE Shield (v2)  ** to be verified **
+ * - RedBearLab BLE Nano ** works with modifications **
+ *
+ *================================================================================================*/
+
+// change this to a unique name per board if running StandardFirmataBLE on multiple boards
+// within the same physical space
+#define FIRMATA_BLE_LOCAL_NAME "FIRMATA"
+
+/*
+ * RedBearLab BLE Shield
+ *
+ * If you are using a RedBearLab BLE shield, uncomment the define below.
+ * Also, change the define for BLE_RST if you have the jumper set to pin 7 rather than pin 4.
+ *
+ * You will need to use the shield with an Arduino Zero, Due, Mega, or other board with sufficient
+ * Flash and RAM. Arduino Uno, Leonardo and other ATmega328p and Atmega32u4 boards to not have
+ * enough memory to run StandardFirmataBLE.
+ *
+ * TODO: verify if this works and with which boards it works.
+ *
+ * Test script: https://gist.github.com/soundanalogous/927360b797574ed50e27
+ */
+//#define REDBEAR_BLE_SHIELD
+
+#ifdef REDBEAR_BLE_SHIELD
+#include <SPI.h>
+#include <BLEPeripheral.h>
+#include "utility/BLEStream.h"
+
+#define BLE_REQ  9
+#define BLE_RDY  8
+#define BLE_RST  4 // 4 or 7 via jumper on shield
+
+BLEStream stream(BLE_REQ, BLE_RDY, BLE_RST);
+#endif
+
+
+/*==================================================================================================
+ * END BLE CONFIGURATION - you should not need to change anything below this line
+ *================================================================================================*/
+
+/*
+ * Arduino 101
+ *
+ * Test script: https://gist.github.com/soundanalogous/927360b797574ed50e27
+ */
+#ifdef _VARIANT_ARDUINO_101_X_
+#include <CurieBLE.h>
+#include "utility/BLEStream.h"
+BLEStream stream;
+#endif
+
+
+/*
+ * RedBearLab BLE Nano (with default switch settings)
+ *
+ * Blocked on this issue: https://github.com/RedBearLab/nRF51822-Arduino/issues/46
+ * Works with modifications. See comments at top of the test script referenced below.
+ * When the RBL nRF51822-Arduino library issue is resolved, this should work witout
+ * any modifications.
+ *
+ * Test script: https://gist.github.com/soundanalogous/d39bb3eb36333a0906df
+ *
+ * Note: If you have changed the solder jumpers on the Nano you may encounter issues since
+ * the pins are currently mapped in Firmata only for the default (factory) jumper settings.
+ */
+// #ifdef BLE_NANO
+// #include <BLEPeripheral.h>
+// #include "utility/BLEStream.h"
+// BLEStream stream;
+// #endif
+
+
+/*
+ * RedBearLab Blend and Blend Micro
+ *
+ * StandardFirmataBLE requires too much Flash and RAM to run on the ATmega32u4-based Blend
+ * and Blend Micro boards. It may work with ConfigurableFirmata selecting only analog and/or
+ * digital I/O.
+ */
+// #if defined(BLEND_MICRO) || defined(BLEND)
+// #include <SPI.h>
+// #include <BLEPeripheral.h>
+// #include "utility/BLEStream.h"
+
+// #define BLE_REQ  6
+// #define BLE_RDY  7
+// #define BLE_RST  4
+
+// BLEStream stream(BLE_REQ, BLE_RDY, BLE_RST);
+// #endif
+
+
+#if defined(BLE_REQ) && defined(BLE_RDY) && defined(BLE_RST)
+#define IS_IGNORE_BLE_PINS(p) ((p) == BLE_REQ || (p) == BLE_RDY || (p) == BLE_RST)
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataChipKIT/StandardFirmataChipKIT.ino	(revision 224)
@@ -0,0 +1,793 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2015 Brian Schmalz. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+#include <SoftPWMServo.h>  // Gives us PWM and Servo on every pin
+#include <Wire.h>
+#include <Firmata.h>
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+SoftServo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* Sets a pin that is in Servo mode to a particular output value
+ * (i.e. pulse width). Different boards may have different ways of
+ * setting servo values, so putting it in a function keeps things cleaner.
+ */
+void servoWrite(byte pin, int value)
+{
+  SoftPWMServoPWMWrite(PIN_TO_PWM(pin), value);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        servoWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          servoWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  /* For chipKIT Pi board, we need to use Serial1. All others just use Serial. */
+#if defined(_BOARD_CHIPKIT_PI_)
+  Serial1.begin(57600);
+  Firmata.begin(Serial1);
+#else
+  Firmata.begin(57600);
+#endif
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/StandardFirmataEthernet.ino	(revision 224)
@@ -0,0 +1,910 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataEthernet is a client implementation. You will need a Firmata client library with
+  a network transport that can act as a server in order to establish a connection between
+  StandardFirmataEthernet and the Firmata client application.
+
+  To use StandardFirmataEthernet you will need to have one of the following
+  boards or shields:
+
+  - Arduino Ethernet shield (or clone)
+  - Arduino Ethernet board (or clone)
+  - Arduino Yun
+
+  Follow the instructions in the ethernetConfig.h file (ethernetConfig.h tab in Arduino IDE) to
+  configure your particular hardware.
+
+  NOTE: If you are using an Arduino Ethernet shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Uno or other ATMega328 boards: (D4, D10, D11, D12, D13)
+  - Arduino Mega: (D4, D10, D50, D51, D52, D53)
+  - Arduino Leonardo: (D4, D10)
+  - Arduino Due: (D4, D10)
+  - Arduino Zero: (D4, D10)
+
+  If you are using an ArduinoEthernet board, the following pins cannot be used (same as Uno):
+  - D4, D10, D11, D12, D13
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+/*
+ * Uncomment the #define SERIAL_DEBUG line below to receive serial output messages relating to your connection
+ * that may help in the event of connection issues. If defined, some boards may not begin executing this sketch
+ * until the Serial console is opened.
+ */
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+// follow the instructions in ethernetConfig.h to configure your particular hardware
+#include "ethernetConfig.h"
+#include "utility/EthernetClientStream.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+/* network */
+#if defined remote_ip && !defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, remote_ip, NULL, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), remote_ip, NULL, remote_port);
+#endif
+#endif
+
+#if !defined remote_ip && defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#endif
+#endif
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+#ifdef YUN_ETHERNET
+  Bridge.begin();
+#else
+#ifdef local_ip
+  Ethernet.begin((uint8_t *)mac, local_ip); //start ethernet
+#else
+  Ethernet.begin((uint8_t *)mac);           //start ethernet using dhcp
+#endif
+#endif
+
+  DEBUG_PRINTLN("connecting...");
+
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+#ifdef WIZ5100_ETHERNET
+  // StandardFirmataEthernet communicates with Ethernet shields over SPI. Therefore all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // add Pin 10 and configure pin 53 as output if using a MEGA with an Ethernet shield.
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_IGNORE_ETHERNET_SHIELD(i)
+  #if defined(__AVR_ATmega32U4__)
+        || 24 == i // On Leonardo, pin 24 maps to D4 and pin 28 maps to D10
+        || 28 == i
+  #endif
+       ) {
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+
+  // Arduino Ethernet and Arduino EthernetShield have SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+#endif // WIZ5100_ETHERNET
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+
+#if !defined local_ip && !defined YUN_ETHERNET
+  // only necessary when using DHCP, ensures local IP is updated appropriately if it changes
+  if (Ethernet.maintain()) {
+    stream.maintain(Ethernet.localIP());
+  }
+#endif
+
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/ethernetConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/ethernetConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernet/ethernetConfig.h	(revision 224)
@@ -0,0 +1,85 @@
+/*==============================================================================
+ * NETWORK CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *
+ * Currently StandardFirmataEthernet is configured as a client. An option to
+ * configure as a server may be added in the future.
+ *============================================================================*/
+
+// STEP 1 [REQUIRED]
+// Uncomment / comment the appropriate set of includes for your hardware (OPTION A or B)
+// Option A is enabled by default.
+
+/*
+ * OPTION A: Configure for Arduino Ethernet board or Arduino Ethernet shield (or clone)
+ *
+ * To configure StandardFirmataEthernet to use the original WIZ5100-based
+ * ethernet shield or Arduino Ethernet uncomment the WIZ5100_ETHERNET define below
+ */
+#define WIZ5100_ETHERNET
+
+#ifdef WIZ5100_ETHERNET
+#include <SPI.h>
+#include <Ethernet.h>
+EthernetClient client;
+#endif
+
+/*
+ * OPTION B: Configure for Arduin Yun
+ *
+ * The Ethernet port on the Arduino Yun board can be used with Firmata in this configuration.
+ *
+ * To execute StandardFirmataEthernet on Yun uncomment the YUN_ETHERNET define below and make
+ * sure the WIZ5100_ETHERNET define (above) is commented out.
+ *
+ * On Yun there's no need to configure local_ip and mac address as this is automatically
+ * configured on the linux-side of Yun.
+ */
+//#define YUN_ETHERNET
+
+#ifdef YUN_ETHERNET
+#include <Bridge.h>
+#include <YunClient.h>
+YunClient client;
+#endif
+
+
+// STEP 2 [REQUIRED for all boards and shields]
+// replace with IP of the server you want to connect to, comment out if using 'remote_host'
+#define remote_ip IPAddress(10, 0, 0, 3)
+// *** REMOTE HOST IS NOT YET WORKING ***
+// replace with hostname of server you want to connect to, comment out if using 'remote_ip'
+// #define remote_host "server.local"
+
+// STEP 3 [REQUIRED unless using Arduin Yun]
+// Replace with the port that your server is listening on
+#define remote_port 3030
+
+// STEP 4 [REQUIRED unless using Arduino Yun OR if not using DHCP]
+// Replace with your board or ethernet shield's IP address
+// Comment out if you want to use DHCP
+#define local_ip IPAddress(10, 0, 0, 15)
+
+// STEP 5 [REQUIRED unless using Arduino Yun]
+// replace with ethernet shield mac. Must be unique for your network
+const byte mac[] = {0x90, 0xA2, 0xDA, 0x00, 0x53, 0xE5};
+
+/*==============================================================================
+ * CONFIGURATION ERROR CHECK (don't change anything here)
+ *============================================================================*/
+
+#if !defined WIZ5100_ETHERNET && !defined YUN_ETHERNET
+#error "you must define either WIZ5100_ETHERNET or YUN_ETHERNET in ethernetConfig.h"
+#endif
+
+#if defined remote_ip && defined remote_host
+#error "cannot define both remote_ip and remote_host at the same time in ethernetConfig.h"
+#endif
+
+/*==============================================================================
+ * PIN IGNORE MACROS (don't change anything here)
+ *============================================================================*/
+
+// ignore SPI pins, pin 10 (Ethernet SS) and pin 4 (SS for SD-Card on Ethernet shield)
+#define IS_IGNORE_ETHERNET_SHIELD(p) ((IS_PIN_SPI(p) || (p) == 4) || (p) == 10)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/StandardFirmataEthernetPlus.ino	(revision 224)
@@ -0,0 +1,909 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataEthernetPlus is a client implementation. You will need a Firmata client library
+  with a network transport that can act as a server in order to establish a connection between
+  StandardFirmataEthernetPlus and the Firmata client application.
+
+  StandardFirmataEthernetPlus adds additional features that may exceed the Flash and
+  RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with a board that
+  has > 32k Flash and > 3k RAM such as: Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2, etc.
+
+  This sketch consumes too much Flash and RAM to run reliably on an
+  Arduino Leonardo, Yun, ATMega32u4-based board. Use StandardFirmataEthernet.ino instead
+  for those boards and other boards that do not meet the Flash and RAM requirements.
+
+  To use StandardFirmataEthernet you will need to have one of the following
+  boards or shields:
+
+  - Arduino Ethernet shield (or clone)
+  - Arduino Ethernet board (or clone)
+
+  Follow the instructions in the ethernetConfig.h file (ethernetConfig.h tab in Arduino IDE) to
+  configure your particular hardware.
+
+  NOTE: If you are using an Arduino Ethernet shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Mega: (D4, D10, D50, D51, D52, D53)
+  - Arduino Due: (D4, D10)
+  - Arduino Zero: (D4, D10)
+  - Arduino Uno or other ATMega328p boards: (D4, D10, D11, D12, D13)
+
+  If you are using an ArduinoEthernet board, the following pins cannot be used (same as Uno):
+  - D4, D10, D11, D12, D13
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+/*
+ * Uncomment the #define SERIAL_DEBUG line below to receive serial output messages relating to your connection
+ * that may help in the event of connection issues. If defined, some boards may not begin executing this sketch
+ * until the Serial console is opened.
+ */
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+// follow the instructions in ethernetConfig.h to configure your particular hardware
+#include "ethernetConfig.h"
+#include "utility/EthernetClientStream.h"
+
+#include "utility/SerialFirmata.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#if defined remote_ip && !defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, remote_ip, NULL, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), remote_ip, NULL, remote_port);
+#endif
+#endif
+
+#if !defined remote_ip && defined remote_host
+#ifdef local_ip
+EthernetClientStream stream(client, local_ip, IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#else
+EthernetClientStream stream(client, IPAddress(0, 0, 0, 0), IPAddress(0, 0, 0, 0), remote_host, remote_port);
+#endif
+#endif
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  DEBUG_BEGIN(9600);
+
+#ifdef local_ip
+  Ethernet.begin((uint8_t *)mac, local_ip); //start ethernet
+#else
+  Ethernet.begin((uint8_t *)mac);           //start ethernet using dhcp
+#endif
+
+  DEBUG_PRINTLN("connecting...");
+
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+#ifdef WIZ5100_ETHERNET
+  // StandardFirmataEthernetPlus communicates with Ethernet shields over SPI. Therefore all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // add Pin 10 and configure pin 53 as output if using a MEGA with an Ethernet shield.
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    if (IS_IGNORE_ETHERNET_SHIELD(i)) {
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+
+  // Arduino Ethernet and Arduino EthernetShield have SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+#endif // WIZ5100_ETHERNET
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+
+#if !defined local_ip
+  // only necessary when using DHCP, ensures local IP is updated appropriately if it changes
+  if (Ethernet.maintain()) {
+    stream.maintain(Ethernet.localIP());
+  }
+#endif
+
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/ethernetConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/ethernetConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataEthernetPlus/ethernetConfig.h	(revision 224)
@@ -0,0 +1,55 @@
+/*==============================================================================
+ * NETWORK CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *
+ * Currently StandardFirmataEthernetPlus is configured as a client. An option to
+ * configure as a server may be added in the future.
+ *============================================================================*/
+
+/*
+ * Only WIZ5100-based shields and boards are currently supported for
+ * StandardFirmataEthernetPlus.
+ */
+#define WIZ5100_ETHERNET
+
+#ifdef WIZ5100_ETHERNET
+#include <SPI.h>
+#include <Ethernet.h>
+EthernetClient client;
+#endif
+
+// STEP 1 [REQUIRED for all boards and shields]
+// replace with IP of the server you want to connect to, comment out if using 'remote_host'
+#define remote_ip IPAddress(10, 0, 0, 3)
+// *** REMOTE HOST IS NOT YET WORKING ***
+// replace with hostname of server you want to connect to, comment out if using 'remote_ip'
+// #define remote_host "server.local"
+
+// STEP 2 [REQUIRED]
+// Replace with the port that your server is listening on
+#define remote_port 3030
+
+// STEP 3 [REQUIRED unless using DHCP]
+// Replace with your board or ethernet shield's IP address
+// Comment out if you want to use DHCP
+#define local_ip IPAddress(10, 0, 0, 15)
+
+// STEP 4 [REQUIRED]
+// replace with ethernet shield mac. Must be unique for your network
+const byte mac[] = {0x90, 0xA2, 0xDA, 0x00, 0x53, 0xE5};
+
+/*==============================================================================
+ * CONFIGURATION ERROR CHECK (don't change anything here)
+ *============================================================================*/
+
+#if defined remote_ip && defined remote_host
+#error "cannot define both remote_ip and remote_host at the same time in ethernetConfig.h"
+#endif
+
+/*==============================================================================
+ * PIN IGNORE MACROS (don't change anything here)
+ *============================================================================*/
+
+// ignore SPI pins, pin 10 (Ethernet SS) and pin 4 (SS for SD-Card on Ethernet shield)
+#define IS_IGNORE_ETHERNET_SHIELD(p) ((IS_PIN_SPI(p) || (p) == 4) || (p) == 10)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/StandardFirmataPlus.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/StandardFirmataPlus.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataPlus/StandardFirmataPlus.ino	(revision 224)
@@ -0,0 +1,838 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataPlus adds additional features that may exceed the Flash and
+  RAM sizes of Arduino boards such as ATMega328p (Uno) and ATMega32u4
+  (Leonardo, Micro, Yun, etc). It is best to use StandardFirmataPlus with higher
+  memory boards such as the Arduino Mega, Arduino Due, Teensy 3.0/3.1/3.2.
+
+  All Firmata examples that are appended with "Plus" add the following features:
+
+  - Ability to interface with serial devices using UART, USART, or SoftwareSerial
+    depending on the capatilities of the board.
+
+  At the time of this writing, StandardFirmataPlus will still compile and run
+  on ATMega328p and ATMega32u4-based boards, but future versions of this sketch
+  may not as new features are added.
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+#include "utility/SerialFirmata.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+/* analog inputs */
+int analogInputsToReport = 0; // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to run the main loop (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous more */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Serial output queue using Serial.print() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void setup()
+{
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // Save a couple of seconds by disabling the startup blink sequence.
+  Firmata.disableBlinkVersion();
+
+  // to use a port other than Serial, such as Serial1 on an Arduino Leonardo or Mega,
+  // Call begin(baud) on the alternate serial port and pass it to Firmata to begin like this:
+  // Serial1.begin(57600);
+  // Firmata.begin(Serial1);
+  // However do not do this if you are using SERIAL_MESSAGE
+
+  Firmata.begin(57600);
+  while (!Serial) {
+    ; // wait for serial port to connect. Needed for ATmega32u4-based boards and Arduino 101
+  }
+
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * FTDI buffer using Serial.print()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available())
+    Firmata.processInput();
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/LICENSE.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/LICENSE.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/LICENSE.txt	(revision 224)
@@ -0,0 +1,466 @@
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+		       Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL.  It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+			    Preamble
+
+  The licenses for most software are designed to take away your
+freedom to share and change it.  By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+  This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it.  You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+  When we speak of free software, we are referring to freedom of use,
+not price.  Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+  To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights.  These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+  For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you.  You must make sure that they, too, receive or can get the source
+code.  If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it.  And you must show them these terms so they know their rights.
+
+  We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+  To protect each distributor, we want to make it very clear that
+there is no warranty for the free library.  Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+
+  Finally, software patents pose a constant threat to the existence of
+any free program.  We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder.  Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+  Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License.  This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License.  We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+  When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library.  The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom.  The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+  We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License.  It also provides other free software developers Less
+of an advantage over competing non-free programs.  These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries.  However, the Lesser license provides advantages in certain
+special circumstances.
+
+  For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard.  To achieve this, non-free programs must be
+allowed to use the library.  A more frequent case is that a free
+library does the same job as widely used non-free libraries.  In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+  In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software.  For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+  Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+  The precise terms and conditions for copying, distribution and
+modification follow.  Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library".  The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+
+		  GNU LESSER GENERAL PUBLIC LICENSE
+   TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+  0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+  A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+  The "Library", below, refers to any such software library or work
+which has been distributed under these terms.  A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language.  (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+  "Source code" for a work means the preferred form of the work for
+making modifications to it.  For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+  Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope.  The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it).  Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+  
+  1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+  You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+
+  2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+    a) The modified work must itself be a software library.
+
+    b) You must cause the files modified to carry prominent notices
+    stating that you changed the files and the date of any change.
+
+    c) You must cause the whole of the work to be licensed at no
+    charge to all third parties under the terms of this License.
+
+    d) If a facility in the modified Library refers to a function or a
+    table of data to be supplied by an application program that uses
+    the facility, other than as an argument passed when the facility
+    is invoked, then you must make a good faith effort to ensure that,
+    in the event an application does not supply such function or
+    table, the facility still operates, and performs whatever part of
+    its purpose remains meaningful.
+
+    (For example, a function in a library to compute square roots has
+    a purpose that is entirely well-defined independent of the
+    application.  Therefore, Subsection 2d requires that any
+    application-supplied function or table used by this function must
+    be optional: if the application does not supply it, the square
+    root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole.  If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works.  But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+  3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library.  To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License.  (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.)  Do not make any other change in
+these notices.
+
+
+  Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+  This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+  4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+  If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+  5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library".  Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+  However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library".  The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+  When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library.  The
+threshold for this to be true is not precisely defined by law.
+
+  If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work.  (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+  Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+
+  6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+  You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License.  You must supply a copy of this License.  If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License.  Also, you must do one
+of these things:
+
+    a) Accompany the work with the complete corresponding
+    machine-readable source code for the Library including whatever
+    changes were used in the work (which must be distributed under
+    Sections 1 and 2 above); and, if the work is an executable linked
+    with the Library, with the complete machine-readable "work that
+    uses the Library", as object code and/or source code, so that the
+    user can modify the Library and then relink to produce a modified
+    executable containing the modified Library.  (It is understood
+    that the user who changes the contents of definitions files in the
+    Library will not necessarily be able to recompile the application
+    to use the modified definitions.)
+
+    b) Use a suitable shared library mechanism for linking with the
+    Library.  A suitable mechanism is one that (1) uses at run time a
+    copy of the library already present on the user's computer system,
+    rather than copying library functions into the executable, and (2)
+    will operate properly with a modified version of the library, if
+    the user installs one, as long as the modified version is
+    interface-compatible with the version that the work was made with.
+
+    c) Accompany the work with a written offer, valid for at
+    least three years, to give the same user the materials
+    specified in Subsection 6a, above, for a charge no more
+    than the cost of performing this distribution.
+
+    d) If distribution of the work is made by offering access to copy
+    from a designated place, offer equivalent access to copy the above
+    specified materials from the same place.
+
+    e) Verify that the user has already received a copy of these
+    materials or that you have already sent this user a copy.
+
+  For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it.  However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+  It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system.  Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+
+  7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+    a) Accompany the combined library with a copy of the same work
+    based on the Library, uncombined with any other library
+    facilities.  This must be distributed under the terms of the
+    Sections above.
+
+    b) Give prominent notice with the combined library of the fact
+    that part of it is a work based on the Library, and explaining
+    where to find the accompanying uncombined form of the same work.
+
+  8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License.  Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License.  However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+  9. You are not required to accept this License, since you have not
+signed it.  However, nothing else grants you permission to modify or
+distribute the Library or its derivative works.  These actions are
+prohibited by law if you do not accept this License.  Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+  10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions.  You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+
+  11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License.  If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all.  For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices.  Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+  12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded.  In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+  13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number.  If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation.  If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+
+  14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission.  For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this.  Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+			    NO WARRANTY
+
+  15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE.  THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU.  SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+  16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/StandardFirmataWiFi.ino	(revision 224)
@@ -0,0 +1,1023 @@
+/*
+  Firmata is a generic protocol for communicating with microcontrollers
+  from software on a host computer. It is intended to work with
+  any host computer software package.
+
+  To download a host software package, please clink on the following link
+  to open the list of Firmata client libraries your default browser.
+
+  https://github.com/firmata/arduino#firmata-client-libraries
+
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+  Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+/*
+  README
+
+  StandardFirmataWiFi is a WiFi server application. You will need a Firmata client library with
+  a network transport in order to establish a connection with StandardFirmataWiFi.
+
+  To use StandardFirmataWiFi you will need to have one of the following
+  boards or shields:
+
+  - Arduino WiFi Shield (or clone)
+  - Arduino WiFi Shield 101
+  - Arduino MKR1000 board (built-in WiFi 101)
+  - Adafruit HUZZAH CC3000 WiFi Shield (support coming soon)
+
+  Follow the instructions in the wifiConfig.h file (wifiConfig.h tab in Arduino IDE) to
+  configure your particular hardware.
+
+  Dependencies:
+  - WiFi Shield 101 requires version 0.7.0 or higher of the WiFi101 library (available in Arduino
+    1.6.8 or higher, or update the library via the Arduino Library Manager or clone from source:
+    https://github.com/arduino-libraries/WiFi101)
+
+  In order to use the WiFi Shield 101 with Firmata you will need a board with at least
+  35k of Flash memory. This means you cannot use the WiFi Shield 101 with an Arduino Uno
+  or any other ATmega328p-based microcontroller or with an Arduino Leonardo or other
+  ATmega32u4-based microcontroller. Some boards that will work are:
+
+  - Arduino Zero
+  - Arduino Due
+  - Arduino 101
+  - Arduino Mega
+
+  NOTE: If you are using an Arduino WiFi (legacy) shield you cannot use the following pins on
+  the following boards. Firmata will ignore any requests to use these pins:
+
+  - Arduino Uno or other ATMega328 boards: (D4, D7, D10, D11, D12, D13)
+  - Arduino Mega: (D4, D7, D10, D50, D51, D52, D53)
+  - Arduino Due, Zero or Leonardo: (D4, D7, D10)
+
+  If you are using an Arduino WiFi 101 shield you cannot use the following pins on the following
+  boards:
+
+  - Arduino Due or Zero: (D5, D7, D10)
+  - Arduino Mega: (D5, D7, D10, D50, D52, D53)
+*/
+
+#include <Servo.h>
+#include <Wire.h>
+#include <Firmata.h>
+
+/*
+ * Uncomment the #define SERIAL_DEBUG line below to receive serial output messages relating to your
+ * connection that may help in the event of connection issues. If defined, some boards may not begin
+ * executing this sketch until the Serial console is opened.
+ */
+//#define SERIAL_DEBUG
+#include "utility/firmataDebug.h"
+
+/*
+ * Uncomment the following include to enable interfacing with Serial devices via hardware or
+ * software serial. Note that if enabled, this sketch will likely consume too much memory to run on
+ * an Arduino Uno or Leonardo or other ATmega328p-based or ATmega32u4-based boards.
+ */
+//#include "utility/SerialFirmata.h"
+
+// follow the instructions in wifiConfig.h to configure your particular hardware
+#include "wifiConfig.h"
+
+#define I2C_WRITE                   B00000000
+#define I2C_READ                    B00001000
+#define I2C_READ_CONTINUOUSLY       B00010000
+#define I2C_STOP_READING            B00011000
+#define I2C_READ_WRITE_MODE_MASK    B00011000
+#define I2C_10BIT_ADDRESS_MODE_MASK B00100000
+#define I2C_END_TX_MASK             B01000000
+#define I2C_STOP_TX                 1
+#define I2C_RESTART_TX              0
+#define I2C_MAX_QUERIES             8
+#define I2C_REGISTER_NOT_SPECIFIED  -1
+
+// the minimum interval for sampling analog input
+#define MINIMUM_SAMPLING_INTERVAL   1
+
+#define WIFI_MAX_CONN_ATTEMPTS      3
+
+/*==============================================================================
+ * GLOBAL VARIABLES
+ *============================================================================*/
+
+#ifdef FIRMATA_SERIAL_FEATURE
+SerialFirmata serialFeature;
+#endif
+
+#ifdef STATIC_IP_ADDRESS
+IPAddress local_ip(STATIC_IP_ADDRESS);
+#endif
+
+int wifiConnectionAttemptCounter = 0;
+int wifiStatus = WL_IDLE_STATUS;
+
+/* analog inputs */
+int analogInputsToReport = 0;      // bitwise array to store pin reporting
+
+/* digital input ports */
+byte reportPINs[TOTAL_PORTS];       // 1 = report this port, 0 = silence
+byte previousPINs[TOTAL_PORTS];     // previous 8 bits sent
+
+/* pins configuration */
+byte portConfigInputs[TOTAL_PORTS]; // each bit: 1 = pin in INPUT, 0 = anything else
+
+/* timer variables */
+unsigned long currentMillis;        // store the current value from millis()
+unsigned long previousMillis;       // for comparison with currentMillis
+unsigned int samplingInterval = 19; // how often to sample analog inputs (in ms)
+
+/* i2c data */
+struct i2c_device_info {
+  byte addr;
+  int reg;
+  byte bytes;
+  byte stopTX;
+};
+
+/* for i2c read continuous mode */
+i2c_device_info query[I2C_MAX_QUERIES];
+
+byte i2cRxData[64];
+boolean isI2CEnabled = false;
+signed char queryIndex = -1;
+// default delay time between i2c read request and Wire.requestFrom()
+unsigned int i2cReadDelayTime = 0;
+
+Servo servos[MAX_SERVOS];
+byte servoPinMap[TOTAL_PINS];
+byte detachedServos[MAX_SERVOS];
+byte detachedServoCount = 0;
+byte servoCount = 0;
+
+boolean isResetting = false;
+
+/* utility functions */
+void wireWrite(byte data)
+{
+#if ARDUINO >= 100
+  Wire.write((byte)data);
+#else
+  Wire.send(data);
+#endif
+}
+
+byte wireRead(void)
+{
+#if ARDUINO >= 100
+  return Wire.read();
+#else
+  return Wire.receive();
+#endif
+}
+
+/*==============================================================================
+ * FUNCTIONS
+ *============================================================================*/
+
+void attachServo(byte pin, int minPulse, int maxPulse)
+{
+  if (servoCount < MAX_SERVOS) {
+    // reuse indexes of detached servos until all have been reallocated
+    if (detachedServoCount > 0) {
+      servoPinMap[pin] = detachedServos[detachedServoCount - 1];
+      if (detachedServoCount > 0) detachedServoCount--;
+    } else {
+      servoPinMap[pin] = servoCount;
+      servoCount++;
+    }
+    if (minPulse > 0 && maxPulse > 0) {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin), minPulse, maxPulse);
+    } else {
+      servos[servoPinMap[pin]].attach(PIN_TO_DIGITAL(pin));
+    }
+  } else {
+    Firmata.sendString("Max servos attached");
+  }
+}
+
+void detachServo(byte pin)
+{
+  servos[servoPinMap[pin]].detach();
+  // if we're detaching the last servo, decrement the count
+  // otherwise store the index of the detached servo
+  if (servoPinMap[pin] == servoCount && servoCount > 0) {
+    servoCount--;
+  } else if (servoCount > 0) {
+    // keep track of detached servos because we want to reuse their indexes
+    // before incrementing the count of attached servos
+    detachedServoCount++;
+    detachedServos[detachedServoCount - 1] = servoPinMap[pin];
+  }
+
+  servoPinMap[pin] = 255;
+}
+
+void readAndReportData(byte address, int theRegister, byte numBytes, byte stopTX) {
+  // allow I2C requests that don't require a register read
+  // for example, some devices using an interrupt pin to signify new data available
+  // do not always require the register read so upon interrupt you call Wire.requestFrom()
+  if (theRegister != I2C_REGISTER_NOT_SPECIFIED) {
+    Wire.beginTransmission(address);
+    wireWrite((byte)theRegister);
+    Wire.endTransmission(stopTX); // default = true
+    // do not set a value of 0
+    if (i2cReadDelayTime > 0) {
+      // delay is necessary for some devices such as WiiNunchuck
+      delayMicroseconds(i2cReadDelayTime);
+    }
+  } else {
+    theRegister = 0;  // fill the register with a dummy value
+  }
+
+  Wire.requestFrom(address, numBytes);  // all bytes are returned in requestFrom
+
+  // check to be sure correct number of bytes were returned by slave
+  if (numBytes < Wire.available()) {
+    Firmata.sendString("I2C: Too many bytes received");
+  } else if (numBytes > Wire.available()) {
+    Firmata.sendString("I2C: Too few bytes received");
+  }
+
+  i2cRxData[0] = address;
+  i2cRxData[1] = theRegister;
+
+  for (int i = 0; i < numBytes && Wire.available(); i++) {
+    i2cRxData[2 + i] = wireRead();
+  }
+
+  // send slave address, register and received bytes
+  Firmata.sendSysex(SYSEX_I2C_REPLY, numBytes + 2, i2cRxData);
+}
+
+void outputPort(byte portNumber, byte portValue, byte forceSend)
+{
+  // pins not configured as INPUT are cleared to zeros
+  portValue = portValue & portConfigInputs[portNumber];
+  // only send if the value is different than previously sent
+  if (forceSend || previousPINs[portNumber] != portValue) {
+    Firmata.sendDigitalPort(portNumber, portValue);
+    previousPINs[portNumber] = portValue;
+  }
+}
+
+/* -----------------------------------------------------------------------------
+ * check all the active digital inputs for change of state, then add any events
+ * to the Stream output queue using Stream.write() */
+void checkDigitalInputs(void)
+{
+  /* Using non-looping code allows constants to be given to readPort().
+   * The compiler will apply substantial optimizations if the inputs
+   * to readPort() are compile-time constants. */
+  if (TOTAL_PORTS > 0 && reportPINs[0]) outputPort(0, readPort(0, portConfigInputs[0]), false);
+  if (TOTAL_PORTS > 1 && reportPINs[1]) outputPort(1, readPort(1, portConfigInputs[1]), false);
+  if (TOTAL_PORTS > 2 && reportPINs[2]) outputPort(2, readPort(2, portConfigInputs[2]), false);
+  if (TOTAL_PORTS > 3 && reportPINs[3]) outputPort(3, readPort(3, portConfigInputs[3]), false);
+  if (TOTAL_PORTS > 4 && reportPINs[4]) outputPort(4, readPort(4, portConfigInputs[4]), false);
+  if (TOTAL_PORTS > 5 && reportPINs[5]) outputPort(5, readPort(5, portConfigInputs[5]), false);
+  if (TOTAL_PORTS > 6 && reportPINs[6]) outputPort(6, readPort(6, portConfigInputs[6]), false);
+  if (TOTAL_PORTS > 7 && reportPINs[7]) outputPort(7, readPort(7, portConfigInputs[7]), false);
+  if (TOTAL_PORTS > 8 && reportPINs[8]) outputPort(8, readPort(8, portConfigInputs[8]), false);
+  if (TOTAL_PORTS > 9 && reportPINs[9]) outputPort(9, readPort(9, portConfigInputs[9]), false);
+  if (TOTAL_PORTS > 10 && reportPINs[10]) outputPort(10, readPort(10, portConfigInputs[10]), false);
+  if (TOTAL_PORTS > 11 && reportPINs[11]) outputPort(11, readPort(11, portConfigInputs[11]), false);
+  if (TOTAL_PORTS > 12 && reportPINs[12]) outputPort(12, readPort(12, portConfigInputs[12]), false);
+  if (TOTAL_PORTS > 13 && reportPINs[13]) outputPort(13, readPort(13, portConfigInputs[13]), false);
+  if (TOTAL_PORTS > 14 && reportPINs[14]) outputPort(14, readPort(14, portConfigInputs[14]), false);
+  if (TOTAL_PORTS > 15 && reportPINs[15]) outputPort(15, readPort(15, portConfigInputs[15]), false);
+}
+
+// -----------------------------------------------------------------------------
+/* sets the pin mode to the correct state and sets the relevant bits in the
+ * two bit-arrays that track Digital I/O and PWM status
+ */
+void setPinModeCallback(byte pin, int mode)
+{
+  if (Firmata.getPinMode(pin) == PIN_MODE_IGNORE)
+    return;
+
+  if (Firmata.getPinMode(pin) == PIN_MODE_I2C && isI2CEnabled && mode != PIN_MODE_I2C) {
+    // disable i2c so pins can be used for other functions
+    // the following if statements should reconfigure the pins properly
+    disableI2CPins();
+  }
+  if (IS_PIN_DIGITAL(pin) && mode != PIN_MODE_SERVO) {
+    if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+      detachServo(pin);
+    }
+  }
+  if (IS_PIN_ANALOG(pin)) {
+    reportAnalogCallback(PIN_TO_ANALOG(pin), mode == PIN_MODE_ANALOG ? 1 : 0); // turn on/off reporting
+  }
+  if (IS_PIN_DIGITAL(pin)) {
+    if (mode == INPUT || mode == PIN_MODE_PULLUP) {
+      portConfigInputs[pin / 8] |= (1 << (pin & 7));
+    } else {
+      portConfigInputs[pin / 8] &= ~(1 << (pin & 7));
+    }
+  }
+  Firmata.setPinState(pin, 0);
+  switch (mode) {
+    case PIN_MODE_ANALOG:
+      if (IS_PIN_ANALOG(pin)) {
+        if (IS_PIN_DIGITAL(pin)) {
+          pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+          // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+          digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        }
+        Firmata.setPinMode(pin, PIN_MODE_ANALOG);
+      }
+      break;
+    case INPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT);    // disable output driver
+#if ARDUINO <= 100
+        // deprecated since Arduino 1.0.1 - TODO: drop support in Firmata 2.6
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable internal pull-ups
+#endif
+        Firmata.setPinMode(pin, INPUT);
+      }
+      break;
+    case PIN_MODE_PULLUP:
+      if (IS_PIN_DIGITAL(pin)) {
+        pinMode(PIN_TO_DIGITAL(pin), INPUT_PULLUP);
+        Firmata.setPinMode(pin, PIN_MODE_PULLUP);
+        Firmata.setPinState(pin, 1);
+      }
+      break;
+    case OUTPUT:
+      if (IS_PIN_DIGITAL(pin)) {
+        digitalWrite(PIN_TO_DIGITAL(pin), LOW); // disable PWM
+        pinMode(PIN_TO_DIGITAL(pin), OUTPUT);
+        Firmata.setPinMode(pin, OUTPUT);
+      }
+      break;
+    case PIN_MODE_PWM:
+      if (IS_PIN_PWM(pin)) {
+        pinMode(PIN_TO_PWM(pin), OUTPUT);
+        analogWrite(PIN_TO_PWM(pin), 0);
+        Firmata.setPinMode(pin, PIN_MODE_PWM);
+      }
+      break;
+    case PIN_MODE_SERVO:
+      if (IS_PIN_DIGITAL(pin)) {
+        Firmata.setPinMode(pin, PIN_MODE_SERVO);
+        if (servoPinMap[pin] == 255 || !servos[servoPinMap[pin]].attached()) {
+          // pass -1 for min and max pulse values to use default values set
+          // by Servo library
+          attachServo(pin, -1, -1);
+        }
+      }
+      break;
+    case PIN_MODE_I2C:
+      if (IS_PIN_I2C(pin)) {
+        // mark the pin as i2c
+        // the user must call I2C_CONFIG to enable I2C for a device
+        Firmata.setPinMode(pin, PIN_MODE_I2C);
+      }
+      break;
+    case PIN_MODE_SERIAL:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handlePinMode(pin, PIN_MODE_SERIAL);
+#endif
+      break;
+    default:
+      Firmata.sendString("Unknown pin mode"); // TODO: put error msgs in EEPROM
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+/*
+ * Sets the value of an individual pin. Useful if you want to set a pin value but
+ * are not tracking the digital port state.
+ * Can only be used on pins configured as OUTPUT.
+ * Cannot be used to enable pull-ups on Digital INPUT pins.
+ */
+void setPinValueCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS && IS_PIN_DIGITAL(pin)) {
+    if (Firmata.getPinMode(pin) == OUTPUT) {
+      Firmata.setPinState(pin, value);
+      digitalWrite(PIN_TO_DIGITAL(pin), value);
+    }
+  }
+}
+
+void analogWriteCallback(byte pin, int value)
+{
+  if (pin < TOTAL_PINS) {
+    switch (Firmata.getPinMode(pin)) {
+      case PIN_MODE_SERVO:
+        if (IS_PIN_DIGITAL(pin))
+          servos[servoPinMap[pin]].write(value);
+        Firmata.setPinState(pin, value);
+        break;
+      case PIN_MODE_PWM:
+        if (IS_PIN_PWM(pin))
+          analogWrite(PIN_TO_PWM(pin), value);
+        Firmata.setPinState(pin, value);
+        break;
+    }
+  }
+}
+
+void digitalWriteCallback(byte port, int value)
+{
+  byte pin, lastPin, pinValue, mask = 1, pinWriteMask = 0;
+
+  if (port < TOTAL_PORTS) {
+    // create a mask of the pins on this port that are writable.
+    lastPin = port * 8 + 8;
+    if (lastPin > TOTAL_PINS) lastPin = TOTAL_PINS;
+    for (pin = port * 8; pin < lastPin; pin++) {
+      // do not disturb non-digital pins (eg, Rx & Tx)
+      if (IS_PIN_DIGITAL(pin)) {
+        // do not touch pins in PWM, ANALOG, SERVO or other modes
+        if (Firmata.getPinMode(pin) == OUTPUT || Firmata.getPinMode(pin) == INPUT) {
+          pinValue = ((byte)value & mask) ? 1 : 0;
+          if (Firmata.getPinMode(pin) == OUTPUT) {
+            pinWriteMask |= mask;
+          } else if (Firmata.getPinMode(pin) == INPUT && pinValue == 1 && Firmata.getPinState(pin) != 1) {
+            // only handle INPUT here for backwards compatibility
+#if ARDUINO > 100
+            pinMode(pin, INPUT_PULLUP);
+#else
+            // only write to the INPUT pin to enable pullups if Arduino v1.0.0 or earlier
+            pinWriteMask |= mask;
+#endif
+          }
+          Firmata.setPinState(pin, pinValue);
+        }
+      }
+      mask = mask << 1;
+    }
+    writePort(port, (byte)value, pinWriteMask);
+  }
+}
+
+
+// -----------------------------------------------------------------------------
+/* sets bits in a bit array (int) to toggle the reporting of the analogIns
+ */
+//void FirmataClass::setAnalogPinReporting(byte pin, byte state) {
+//}
+void reportAnalogCallback(byte analogPin, int value)
+{
+  if (analogPin < TOTAL_ANALOG_PINS) {
+    if (value == 0) {
+      analogInputsToReport = analogInputsToReport & ~ (1 << analogPin);
+    } else {
+      analogInputsToReport = analogInputsToReport | (1 << analogPin);
+      // prevent during system reset or all analog pin values will be reported
+      // which may report noise for unconnected analog pins
+      if (!isResetting) {
+        // Send pin value immediately. This is helpful when connected via
+        // ethernet, wi-fi or bluetooth so pin states can be known upon
+        // reconnecting.
+        Firmata.sendAnalog(analogPin, analogRead(analogPin));
+      }
+    }
+  }
+  // TODO: save status to EEPROM here, if changed
+}
+
+void reportDigitalCallback(byte port, int value)
+{
+  if (port < TOTAL_PORTS) {
+    reportPINs[port] = (byte)value;
+    // Send port value immediately. This is helpful when connected via
+    // ethernet, wi-fi or bluetooth so pin states can be known upon
+    // reconnecting.
+    if (value) outputPort(port, readPort(port, portConfigInputs[port]), true);
+  }
+  // do not disable analog reporting on these 8 pins, to allow some
+  // pins used for digital, others analog.  Instead, allow both types
+  // of reporting to be enabled, but check if the pin is configured
+  // as analog when sampling the analog inputs.  Likewise, while
+  // scanning digital pins, portConfigInputs will mask off values from any
+  // pins configured as analog
+}
+
+/*==============================================================================
+ * SYSEX-BASED commands
+ *============================================================================*/
+
+void sysexCallback(byte command, byte argc, byte *argv)
+{
+  byte mode;
+  byte stopTX;
+  byte slaveAddress;
+  byte data;
+  int slaveRegister;
+  unsigned int delayTime;
+
+  switch (command) {
+    case I2C_REQUEST:
+      mode = argv[1] & I2C_READ_WRITE_MODE_MASK;
+      if (argv[1] & I2C_10BIT_ADDRESS_MODE_MASK) {
+        Firmata.sendString("10-bit addressing not supported");
+        return;
+      }
+      else {
+        slaveAddress = argv[0];
+      }
+
+      // need to invert the logic here since 0 will be default for client
+      // libraries that have not updated to add support for restart tx
+      if (argv[1] & I2C_END_TX_MASK) {
+        stopTX = I2C_RESTART_TX;
+      }
+      else {
+        stopTX = I2C_STOP_TX; // default
+      }
+
+      switch (mode) {
+        case I2C_WRITE:
+          Wire.beginTransmission(slaveAddress);
+          for (byte i = 2; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            wireWrite(data);
+          }
+          Wire.endTransmission();
+          delayMicroseconds(70);
+          break;
+        case I2C_READ:
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          readAndReportData(slaveAddress, (int)slaveRegister, data, stopTX);
+          break;
+        case I2C_READ_CONTINUOUSLY:
+          if ((queryIndex + 1) >= I2C_MAX_QUERIES) {
+            // too many queries, just ignore
+            Firmata.sendString("too many queries");
+            break;
+          }
+          if (argc == 6) {
+            // a slave register is specified
+            slaveRegister = argv[2] + (argv[3] << 7);
+            data = argv[4] + (argv[5] << 7);  // bytes to read
+          }
+          else {
+            // a slave register is NOT specified
+            slaveRegister = (int)I2C_REGISTER_NOT_SPECIFIED;
+            data = argv[2] + (argv[3] << 7);  // bytes to read
+          }
+          queryIndex++;
+          query[queryIndex].addr = slaveAddress;
+          query[queryIndex].reg = slaveRegister;
+          query[queryIndex].bytes = data;
+          query[queryIndex].stopTX = stopTX;
+          break;
+        case I2C_STOP_READING:
+          byte queryIndexToSkip;
+          // if read continuous mode is enabled for only 1 i2c device, disable
+          // read continuous reporting for that device
+          if (queryIndex <= 0) {
+            queryIndex = -1;
+          } else {
+            queryIndexToSkip = 0;
+            // if read continuous mode is enabled for multiple devices,
+            // determine which device to stop reading and remove it's data from
+            // the array, shifiting other array data to fill the space
+            for (byte i = 0; i < queryIndex + 1; i++) {
+              if (query[i].addr == slaveAddress) {
+                queryIndexToSkip = i;
+                break;
+              }
+            }
+
+            for (byte i = queryIndexToSkip; i < queryIndex + 1; i++) {
+              if (i < I2C_MAX_QUERIES) {
+                query[i].addr = query[i + 1].addr;
+                query[i].reg = query[i + 1].reg;
+                query[i].bytes = query[i + 1].bytes;
+                query[i].stopTX = query[i + 1].stopTX;
+              }
+            }
+            queryIndex--;
+          }
+          break;
+        default:
+          break;
+      }
+      break;
+    case I2C_CONFIG:
+      delayTime = (argv[0] + (argv[1] << 7));
+
+      if (delayTime > 0) {
+        i2cReadDelayTime = delayTime;
+      }
+
+      if (!isI2CEnabled) {
+        enableI2CPins();
+      }
+
+      break;
+    case SERVO_CONFIG:
+      if (argc > 4) {
+        // these vars are here for clarity, they'll optimized away by the compiler
+        byte pin = argv[0];
+        int minPulse = argv[1] + (argv[2] << 7);
+        int maxPulse = argv[3] + (argv[4] << 7);
+
+        if (IS_PIN_DIGITAL(pin)) {
+          if (servoPinMap[pin] < MAX_SERVOS && servos[servoPinMap[pin]].attached()) {
+            detachServo(pin);
+          }
+          attachServo(pin, minPulse, maxPulse);
+          setPinModeCallback(pin, PIN_MODE_SERVO);
+        }
+      }
+      break;
+    case SAMPLING_INTERVAL:
+      if (argc > 1) {
+        samplingInterval = argv[0] + (argv[1] << 7);
+        if (samplingInterval < MINIMUM_SAMPLING_INTERVAL) {
+          samplingInterval = MINIMUM_SAMPLING_INTERVAL;
+        }
+      } else {
+        //Firmata.sendString("Not enough data");
+      }
+      break;
+    case EXTENDED_ANALOG:
+      if (argc > 1) {
+        int val = argv[1];
+        if (argc > 2) val |= (argv[2] << 7);
+        if (argc > 3) val |= (argv[3] << 14);
+        analogWriteCallback(argv[0], val);
+      }
+      break;
+    case CAPABILITY_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(CAPABILITY_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write((byte)INPUT);
+          Firmata.write(1);
+          Firmata.write((byte)PIN_MODE_PULLUP);
+          Firmata.write(1);
+          Firmata.write((byte)OUTPUT);
+          Firmata.write(1);
+        }
+        if (IS_PIN_ANALOG(pin)) {
+          Firmata.write(PIN_MODE_ANALOG);
+          Firmata.write(10); // 10 = 10-bit resolution
+        }
+        if (IS_PIN_PWM(pin)) {
+          Firmata.write(PIN_MODE_PWM);
+          Firmata.write(8); // 8 = 8-bit resolution
+        }
+        if (IS_PIN_DIGITAL(pin)) {
+          Firmata.write(PIN_MODE_SERVO);
+          Firmata.write(14);
+        }
+        if (IS_PIN_I2C(pin)) {
+          Firmata.write(PIN_MODE_I2C);
+          Firmata.write(1);  // TODO: could assign a number to map to SCL or SDA
+        }
+#ifdef FIRMATA_SERIAL_FEATURE
+        serialFeature.handleCapability(pin);
+#endif
+        Firmata.write(127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+    case PIN_STATE_QUERY:
+      if (argc > 0) {
+        byte pin = argv[0];
+        Firmata.write(START_SYSEX);
+        Firmata.write(PIN_STATE_RESPONSE);
+        Firmata.write(pin);
+        if (pin < TOTAL_PINS) {
+          Firmata.write(Firmata.getPinMode(pin));
+          Firmata.write((byte)Firmata.getPinState(pin) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xFF80) Firmata.write((byte)(Firmata.getPinState(pin) >> 7) & 0x7F);
+          if (Firmata.getPinState(pin) & 0xC000) Firmata.write((byte)(Firmata.getPinState(pin) >> 14) & 0x7F);
+        }
+        Firmata.write(END_SYSEX);
+      }
+      break;
+    case ANALOG_MAPPING_QUERY:
+      Firmata.write(START_SYSEX);
+      Firmata.write(ANALOG_MAPPING_RESPONSE);
+      for (byte pin = 0; pin < TOTAL_PINS; pin++) {
+        Firmata.write(IS_PIN_ANALOG(pin) ? PIN_TO_ANALOG(pin) : 127);
+      }
+      Firmata.write(END_SYSEX);
+      break;
+
+    case SERIAL_MESSAGE:
+#ifdef FIRMATA_SERIAL_FEATURE
+      serialFeature.handleSysex(command, argc, argv);
+#endif
+      break;
+  }
+}
+
+void enableI2CPins()
+{
+  byte i;
+  // is there a faster way to do this? would probaby require importing
+  // Arduino.h to get SCL and SDA pins
+  for (i = 0; i < TOTAL_PINS; i++) {
+    if (IS_PIN_I2C(i)) {
+      // mark pins as i2c so they are ignore in non i2c data requests
+      setPinModeCallback(i, PIN_MODE_I2C);
+    }
+  }
+
+  isI2CEnabled = true;
+
+  Wire.begin();
+}
+
+/* disable the i2c pins so they can be used for other functions */
+void disableI2CPins() {
+  isI2CEnabled = false;
+  // disable read continuous mode for all devices
+  queryIndex = -1;
+}
+
+/*==============================================================================
+ * SETUP()
+ *============================================================================*/
+
+void systemResetCallback()
+{
+  isResetting = true;
+
+  // initialize a defalt state
+  // TODO: option to load config from EEPROM instead of default
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.reset();
+#endif
+
+  if (isI2CEnabled) {
+    disableI2CPins();
+  }
+
+  for (byte i = 0; i < TOTAL_PORTS; i++) {
+    reportPINs[i] = false;    // by default, reporting off
+    portConfigInputs[i] = 0;  // until activated
+    previousPINs[i] = 0;
+  }
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+    // pins with analog capability default to analog input
+    // otherwise, pins default to digital output
+    if (IS_PIN_ANALOG(i)) {
+      // turns off pullup, configures everything
+      setPinModeCallback(i, PIN_MODE_ANALOG);
+    } else if (IS_PIN_DIGITAL(i)) {
+      // sets the output to 0, configures portConfigInputs
+      setPinModeCallback(i, OUTPUT);
+    }
+
+    servoPinMap[i] = 255;
+  }
+  // by default, do not report any analog inputs
+  analogInputsToReport = 0;
+
+  detachedServoCount = 0;
+  servoCount = 0;
+
+  /* send digital inputs to set the initial state on the host computer,
+   * since once in the loop(), this firmware will only send on change */
+  /*
+  TODO: this can never execute, since no pins default to digital input
+        but it will be needed when/if we support EEPROM stored config
+  for (byte i=0; i < TOTAL_PORTS; i++) {
+    outputPort(i, readPort(i, portConfigInputs[i]), true);
+  }
+  */
+  isResetting = false;
+}
+
+void printWifiStatus() {
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+  if ( WiFi.status() != WL_CONNECTED )
+  {
+    DEBUG_PRINT( "WiFi connection failed. Status value: " );
+    DEBUG_PRINTLN( WiFi.status() );
+  }
+  else
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+  {
+    // print the SSID of the network you're attached to:
+    DEBUG_PRINT( "SSID: " );
+
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+    DEBUG_PRINTLN( WiFi.SSID() );
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+
+    // print your WiFi shield's IP address:
+    DEBUG_PRINT( "IP Address: " );
+
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+    IPAddress ip = WiFi.localIP();
+    DEBUG_PRINTLN( ip );
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+
+    // print the received signal strength:
+    DEBUG_PRINT( "signal strength (RSSI): " );
+
+#if defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+    long rssi = WiFi.RSSI();
+    DEBUG_PRINT( rssi );
+#endif    //defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101)
+
+    DEBUG_PRINTLN( " dBm" );
+  }
+}
+
+void setup()
+{
+  /*
+   * WIFI SETUP
+   */
+  DEBUG_BEGIN(9600);
+
+  /*
+   * This statement will clarify how a connection is being made
+   */
+  DEBUG_PRINT( "StandardFirmataWiFi will attempt a WiFi connection " );
+#if defined(WIFI_101)
+  DEBUG_PRINTLN( "using the WiFi 101 library." );
+#elif defined(ARDUINO_WIFI_SHIELD)
+  DEBUG_PRINTLN( "using the legacy WiFi library." );
+#elif defined(HUZZAH_WIFI)
+  DEBUG_PRINTLN( "using the HUZZAH WiFi library." );
+  //else should never happen here as error-checking in wifiConfig.h will catch this
+#endif  //defined(WIFI_101)
+
+  /*
+   * Configure WiFi IP Address
+   */
+#ifdef STATIC_IP_ADDRESS
+  DEBUG_PRINT( "Using static IP: " );
+  DEBUG_PRINTLN( local_ip );
+  //you can also provide a static IP in the begin() functions, but this simplifies
+  //ifdef logic in this sketch due to support for all different encryption types.
+  stream.config( local_ip );
+#else
+  DEBUG_PRINTLN( "IP will be requested from DHCP ..." );
+#endif
+
+  /*
+   * Configure WiFi security
+   */
+#if defined(WIFI_WEP_SECURITY)
+  while (wifiStatus != WL_CONNECTED) {
+    DEBUG_PRINT( "Attempting to connect to WEP SSID: " );
+    DEBUG_PRINTLN(ssid);
+    wifiStatus = stream.begin( ssid, wep_index, wep_key, SERVER_PORT );
+    delay(5000); // TODO - determine minimum delay
+    if (++wifiConnectionAttemptCounter > WIFI_MAX_CONN_ATTEMPTS) break;
+  }
+
+#elif defined(WIFI_WPA_SECURITY)
+  while (wifiStatus != WL_CONNECTED) {
+    DEBUG_PRINT( "Attempting to connect to WPA SSID: " );
+    DEBUG_PRINTLN(ssid);
+    wifiStatus = stream.begin(ssid, wpa_passphrase, SERVER_PORT);
+    delay(5000); // TODO - determine minimum delay
+    if (++wifiConnectionAttemptCounter > WIFI_MAX_CONN_ATTEMPTS) break;
+  }
+
+#else                          //OPEN network
+  while (wifiStatus != WL_CONNECTED) {
+    DEBUG_PRINTLN( "Attempting to connect to open SSID: " );
+    DEBUG_PRINTLN(ssid);
+    wifiStatus = stream.begin(ssid, SERVER_PORT);
+    delay(5000); // TODO - determine minimum delay
+    if (++wifiConnectionAttemptCounter > WIFI_MAX_CONN_ATTEMPTS) break;
+  }
+#endif //defined(WIFI_WEP_SECURITY)
+
+  DEBUG_PRINTLN( "WiFi setup done" );
+  printWifiStatus();
+
+  /*
+   * FIRMATA SETUP
+   */
+  Firmata.setFirmwareVersion(FIRMATA_FIRMWARE_MAJOR_VERSION, FIRMATA_FIRMWARE_MINOR_VERSION);
+
+  Firmata.attach(ANALOG_MESSAGE, analogWriteCallback);
+  Firmata.attach(DIGITAL_MESSAGE, digitalWriteCallback);
+  Firmata.attach(REPORT_ANALOG, reportAnalogCallback);
+  Firmata.attach(REPORT_DIGITAL, reportDigitalCallback);
+  Firmata.attach(SET_PIN_MODE, setPinModeCallback);
+  Firmata.attach(SET_DIGITAL_PIN_VALUE, setPinValueCallback);
+  Firmata.attach(START_SYSEX, sysexCallback);
+  Firmata.attach(SYSTEM_RESET, systemResetCallback);
+
+  // StandardFirmataWiFi communicates with WiFi shields over SPI. Therefore all
+  // SPI pins must be set to IGNORE. Otherwise Firmata would break SPI communication.
+  // Additional pins may also need to be ignored depending on the particular board or
+  // shield in use.
+
+  for (byte i = 0; i < TOTAL_PINS; i++) {
+#if defined(ARDUINO_WIFI_SHIELD)
+    if (IS_IGNORE_WIFI_SHIELD(i)
+  #if defined(__AVR_ATmega32U4__)
+        || 24 == i // On Leonardo, pin 24 maps to D4 and pin 28 maps to D10
+        || 28 == i
+  #endif  //defined(__AVR_ATmega32U4__)
+       ) {
+#elif defined (WIFI_101)
+    if (IS_IGNORE_WIFI101_SHIELD(i)) {
+#elif defined (HUZZAH_WIFI)
+    // TODO
+    if (false) {
+#else
+    if (false) {
+#endif
+      Firmata.setPinMode(i, PIN_MODE_IGNORE);
+    }
+  }
+
+  //Set up controls for the Arduino WiFi Shield SS for the SD Card
+#ifdef ARDUINO_WIFI_SHIELD
+  // Arduino WiFi, Arduino WiFi Shield and Arduino Yun all have SD SS wired to D4
+  pinMode(PIN_TO_DIGITAL(4), OUTPUT);    // switch off SD card bypassing Firmata
+  digitalWrite(PIN_TO_DIGITAL(4), HIGH); // SS is active low;
+
+#if defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+  pinMode(PIN_TO_DIGITAL(53), OUTPUT); // configure hardware SS as output on MEGA
+#endif  //defined(__AVR_ATmega1280__) || defined(__AVR_ATmega2560__)
+
+#endif  //ARDUINO_WIFI_SHIELD
+
+  // start up Network Firmata:
+  Firmata.begin(stream);
+  systemResetCallback();  // reset to default config
+}
+
+/*==============================================================================
+ * LOOP()
+ *============================================================================*/
+void loop()
+{
+  byte pin, analogPin;
+
+  /* DIGITALREAD - as fast as possible, check for changes and output them to the
+   * Stream buffer using Stream.write()  */
+  checkDigitalInputs();
+
+  /* STREAMREAD - processing incoming messagse as soon as possible, while still
+   * checking digital inputs.  */
+  while (Firmata.available()) {
+    Firmata.processInput();
+  }
+
+  // TODO - ensure that Stream buffer doesn't go over 60 bytes
+
+  currentMillis = millis();
+  if (currentMillis - previousMillis > samplingInterval) {
+    previousMillis += samplingInterval;
+    /* ANALOGREAD - do all analogReads() at the configured sampling interval */
+    for (pin = 0; pin < TOTAL_PINS; pin++) {
+      if (IS_PIN_ANALOG(pin) && Firmata.getPinMode(pin) == PIN_MODE_ANALOG) {
+        analogPin = PIN_TO_ANALOG(pin);
+        if (analogInputsToReport & (1 << analogPin)) {
+          Firmata.sendAnalog(analogPin, analogRead(analogPin));
+        }
+      }
+    }
+    // report i2c data for all device with read continuous mode enabled
+    if (queryIndex > -1) {
+      for (byte i = 0; i < queryIndex + 1; i++) {
+        readAndReportData(query[i].addr, query[i].reg, query[i].bytes, query[i].stopTX);
+      }
+    }
+  }
+
+#ifdef FIRMATA_SERIAL_FEATURE
+  serialFeature.update();
+#endif
+
+  stream.maintain();
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/wifiConfig.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/wifiConfig.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/examples/StandardFirmataWiFi/wifiConfig.h	(revision 224)
@@ -0,0 +1,155 @@
+/*==============================================================================
+ * WIFI CONFIGURATION
+ *
+ * You must configure your particular hardware. Follow the steps below.
+ *
+ * Currently StandardFirmataWiFi is configured as a server. An option to
+ * configure as a client may be added in the future.
+ *============================================================================*/
+
+// STEP 1 [REQUIRED]
+// Uncomment / comment the appropriate set of includes for your hardware (OPTION A, B or C)
+// Option A is enabled by default.
+
+/*
+ * OPTION A: Configure for Arduino WiFi shield
+ *
+ * This will configure StandardFirmataWiFi to use the original WiFi library (deprecated) provided
+ * with the Arduino IDE. It is supported by the Arduino WiFi shield (a discontinued product) and
+ * is compatible with 802.11 B/G networks.
+ *
+ * To configure StandardFirmataWiFi to use the Arduino WiFi shield
+ * leave the #define below uncommented.
+ */
+#define ARDUINO_WIFI_SHIELD
+
+//do not modify these next 4 lines
+#ifdef ARDUINO_WIFI_SHIELD
+#include "utility/WiFiStream.h"
+WiFiStream stream;
+#endif
+
+/*
+ * OPTION B: Configure for WiFi 101
+ *
+ * This will configure StandardFirmataWiFi to use the WiFi101 library, which works with the Arduino WiFi101
+ * shield and devices that have the WiFi101 chip built in (such as the MKR1000). It is compatible
+ * with 802.11 B/G/N networks.
+ *
+ * To enable, uncomment the #define WIFI_101 below and verify the #define values under
+ * options A and C are commented out.
+ *
+ * IMPORTANT: You must have the WiFI 101 library installed. To easily install this library, opent the library manager via:
+ * Arduino IDE Menus: Sketch > Include Library > Manage Libraries > filter search for "WiFi101" > Select the result and click 'install'
+ */
+//#define WIFI_101
+
+//do not modify these next 4 lines
+#ifdef WIFI_101
+#include "utility/WiFi101Stream.h"
+WiFi101Stream stream;
+#endif
+
+/*
+ * OPTION C: Configure for HUZZAH
+ *
+ * HUZZAH is not yet supported, this will be added in a later revision to StandardFirmataWiFi
+ */
+
+//------------------------------
+// TODO
+//------------------------------
+//#define HUZZAH_WIFI
+
+
+// STEP 2 [REQUIRED for all boards and shields]
+// replace this with your wireless network SSID
+char ssid[] = "your_network_name";
+
+// STEP 3 [OPTIONAL for all boards and shields]
+// if you want to use a static IP (v4) address, uncomment the line below. You can also change the IP.
+// if this line is commented out, the WiFi shield will attempt to get an IP from the DHCP server
+// #define STATIC_IP_ADDRESS 192,168,1,113
+
+// STEP 4 [REQUIRED for all boards and shields]
+// define your port number here, you will need this to open a TCP connection to your Arduino
+#define SERVER_PORT 3030
+
+// STEP 5 [REQUIRED for all boards and shields]
+// determine your network security type (OPTION A, B, or C). Option A is the most common, and the default.
+
+
+/*
+ * OPTION A: WPA / WPA2
+ *
+ * WPA is the most common network security type. A passphrase is required to connect to this type.
+ *
+ * To enable, leave #define WIFI_WPA_SECURITY uncommented below, set your wpa_passphrase value appropriately,
+ * and do not uncomment the #define values under options B and C
+ */
+#define WIFI_WPA_SECURITY
+
+#ifdef WIFI_WPA_SECURITY
+char wpa_passphrase[] = "your_wpa_passphrase";
+#endif  //WIFI_WPA_SECURITY
+
+/*
+ * OPTION B: WEP
+ *
+ * WEP is a less common (and regarded as less safe) security type. A WEP key and its associated index are required
+ * to connect to this type.
+ *
+ * To enable, Uncomment the #define below, set your wep_index and wep_key values appropriately, and verify
+ * the #define values under options A and C are commented out.
+ */
+//#define WIFI_WEP_SECURITY
+
+#ifdef WIFI_WEP_SECURITY
+//The wep_index below is a zero-indexed value.
+//Valid indices are [0-3], even if your router/gateway numbers your keys [1-4].
+byte wep_index = 0;
+char wep_key[] = "your_wep_key";
+#endif  //WIFI_WEP_SECURITY
+
+
+/*
+ * OPTION C: Open network (no security)
+ *
+ * Open networks have no security, can be connected to by any device that knows the ssid, and are unsafe.
+ *
+ * To enable, uncomment #define WIFI_NO_SECURITY below and verify the #define values
+ * under options A and B are commented out.
+ */
+//#define WIFI_NO_SECURITY
+
+/*==============================================================================
+ * CONFIGURATION ERROR CHECK (don't change anything here)
+ *============================================================================*/
+
+#if ((defined(ARDUINO_WIFI_SHIELD) && (defined(WIFI_101) || defined(HUZZAH_WIFI))) || (defined(WIFI_101) && defined(HUZZAH_WIFI)))
+#error "you may not define more than one wifi device type in wifiConfig.h."
+#endif //WIFI device type check
+
+#if !(defined(ARDUINO_WIFI_SHIELD) || defined(WIFI_101) || defined(HUZZAH_WIFI))
+#error "you must define a wifi device type in wifiConfig.h."
+#endif
+
+#if ((defined(WIFI_NO_SECURITY) && (defined(WIFI_WEP_SECURITY) || defined(WIFI_WPA_SECURITY))) || (defined(WIFI_WEP_SECURITY) && defined(WIFI_WPA_SECURITY)))
+#error "you may not define more than one security type at the same time in wifiConfig.h."
+#endif  //WIFI_* security define check
+
+#if !(defined(WIFI_NO_SECURITY) || defined(WIFI_WEP_SECURITY) || defined(WIFI_WPA_SECURITY))
+#error "you must define a wifi security type in wifiConfig.h."
+#endif  //WIFI_* security define check
+
+/*==============================================================================
+ * PIN IGNORE MACROS (don't change anything here)
+ *============================================================================*/
+
+// ignore SPI pins, pin 5 (reset WiFi101 shield), pin 7 (WiFi handshake) and pin 10 (WiFi SS)
+// also don't ignore SS pin if it's not pin 10
+// TODO - need to differentiate between Arduino WiFi1 101 Shield and Arduino MKR1000
+#define IS_IGNORE_WIFI101_SHIELD(p)  ((p) == 10 || (IS_PIN_SPI(p) && (p) != SS) || (p) == 5 || (p) == 7)
+
+// ignore SPI pins, pin 4 (SS for SD-Card on WiFi-shield), pin 7 (WiFi handshake) and pin 10 (WiFi SS)
+#define IS_IGNORE_WIFI_SHIELD(p)     ((IS_PIN_SPI(p) || (p) == 4) || (p) == 7 || (p) == 10)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/extras/revisions.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/extras/revisions.txt	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/extras/revisions.txt	(revision 224)
@@ -0,0 +1,202 @@
+FIRMATA 2.5.2 - Feb 15, 2016
+
+[core library]
+* Added Wi-Fi transport (Jesse Frush)
+* Added support for Arduino MKR1000 (Jesse Frush)
+* Moved Serial feature to own class SerialFirmata
+* Moved pin config and pin state handling to Firmata.cpp
+* Added new method disableBlinkVersion to provide a way to optionally bypass startup blink sequence
+
+[StandardFirmata & variants]
+* Added StandardFirmataWiFi (Jesse Frush)
+* Added ethernetConfig.h for StandardFirmataEthernet and StandardFirmtaEthernetPlus
+* Removed serialUtils.h and using SerialFirmata class instead for Serial feature
+
+FIRMATA 2.5.1 - Dec 26, 2015
+
+[core library]
+* Added support for Arduino 101
+* Make VERSION_BLINK_PIN optional
+* Separate protocol version from firmware version.
+  Use FIRMATA_PROTOCOL_VERSION_[MAJOR/MINOR/BUGFIX] for protocol and use
+  FIRMATA_FIRMWARE_VERSION_[MAJOR/MINOR/BUGFIX] for firmware (library version).
+
+[StandardFirmata & variants]
+* Added ability to auto-restart I2C transmission by setting bit 6 of byte 3
+  of the I2C_REQUEST message.
+
+FIRMATA 2.5.0 - Nov 7, 2015
+
+[core library]
+* Added Serial feature for interfacing with serial devices via hardware
+  or software serial. See github.com/firmata/protocol/serial.md for details
+* Added ability to set the value of a pin by sending a single value instead
+  of a port value. See 'set digital pin value' in github.com/firmata/protocol/protocol.md
+  for details
+* Added support for Arduino Zero board
+* Added support for Teensy LC board (copied from Teensy Firmata lib)
+* Added support for Pinoccio Scout board (Pawel Szymczykowski)
+* Lowered minimun sampling interval from 10 to 1 millisecond
+* Added new pin mode (PIN_MODE_PULLUP) for setting the INPUT_PULLUP pin mode
+* Changed pin mode defines to safer names (old names still included but
+  deprecated) - see Firmata.h
+
+[StandardFirmata & variants]
+* Created new StandardFirmataPlus that implements the Serial feature
+  Note: The new Serial features is only implemented in the "Plus" versions of
+  StandardFirmata.
+* Created new StandardFirmataEthernetPlus that implements the Serial feature
+* Fixed issue where StandardFirmata was not compiling for Intel Galileo boards
+* Moved StandardFirmataYun to its own repo (github.com/firmata/StandardFirmataYun)
+
+FIRMATA 2.4.4 - Aug 9, 2015
+
+[core library]
+* Added support for chipKIT boards (Brian Schmalz, Rick Anderson and Keith Vogel)
+* Added support for ATmega328 boards (previously only ATmega328p was supported)
+
+[StandardFirmata]
+* Added StandardFirmataChipKIT for ChipKIT boards (Brian Schmalz, Rick Anderson and Keith Vogel)
+* Ensure Serial is ready on Leonardo and other ATMega32u4-based boards
+
+FIRMATA 2.4.3 - Apr 11, 2015
+
+[core library]
+* Added debug macros (utility/firmataDebug.h)
+* Added Norbert Truchsess' EthernetClientStream lib from the configurable branch
+
+[examples]
+* Added StandardFirmataEthernet to enable Firmata over Ethernet
+* Minor updates to StandardFirmata and StandardFirmataYun
+
+FIRMATA 2.4.2 - Mar 16, 2015
+
+[core library]
+* Add support for Teesy 3.1 (Olivier Louvignes)
+
+FIRMATA 2.4.1 - Feb 7, 2015
+
+[core library]
+* Fixed off-by-one bug in setFirmwareNameAndVersion (Brian Schmalz)
+
+[StandardFirmata]
+* Prevent analog values from being reported during system reset
+
+FIRMATA 2.4.0 - Dec 21, 2014
+
+Changes from 2.3.6 to 2.4 that may impact existing Firmata client implementations:
+
+* When sending a string from the client application to the board (STRING_DATA) a
+static buffer is now used for the incoming string in place of a dynamically allocated
+block of memory (see Firmata.cpp lines 181 - 205). In Firmata 2.3.6 and older,
+the dynamically allocated block was never freed, causing a memory leak. If your
+client library had freed this memory in the string callback method, that code
+will break in Firmata 2.4. If the string data needs to persist beyond the string
+callback, it should be copied within the string callback.
+
+* As of Firmata 2.4, when digital port reporting or analog pin reporting is enabled,
+the value of the port (digital) or pin (analog) is immediately sent back to the client
+application. This will likely not have a negative impact on existing client
+implementations, but may be unexpected. This feature was added to better support
+non-serial streams (such as Ethernet, Wi-Fi, Bluetooth, etc) that may lose
+connectivity and need a quick way to get the current state of the pins upon
+reestablishing a connection.
+
+[core library]
+* Changed sendValueAsTwo7bitBytes, startSysex and endSysex from private to
+  public methods.
+* Added Intel Galileo to Boards.h
+* Renamed FirmataSerial to FirmataStream
+* Updated to latest Arduino library format
+* writePort function in Boards.h now returns 1 (to suppress compiler warning)
+* Updated syntax highlighting (keywords.txt)
+* Fixed IS_PIN_SPI ifndef condition in boards.h
+* Added constants to Firmata.h to reserve configurable firmata features
+* Fixed issue where firmwareName was not reported correctly in Windows
+* Ensure incoming String via STRING_DATA command is null-terminated
+* Fixed memory leak when receiving String via STRING_DATA command
+  (note this change may break existing code if you were manually deallocating
+  the incoming string in your string callback function. See code for details)
+* Added ability for user to specify a filename when calling setFirmwareNameAndVersion
+* Increased input data buffer size from 32 to 64 bytes
+
+[StandardFirmata]
+* Updated I2C_READ_CONTINUOUSLY to work with or without slaveRegister (Rick Waldron)
+* Added Yun variant of StandardFirmata
+* When a digital port is enabled, its value is now immediately sent to the client
+* When an analog pin is enabled, its value is now immediately sent to the client
+* Changed the way servo pins are mapped to enable use of servos on
+  a wider range of pins, including analog pins.
+* Fixed management of unexpected sized I2C replies (Nahuel Greco)
+* Fixed a bug when removing a monitored device with I2C_STOP_Reading (Nahuel Greco)
+* Fixed conditional expression in I2C_STOP_READING case
+* Changed samplingInterval from type int to type unsigned int
+* Shortened error message strings to save a few bytes
+
+[examples]
+* Updated FirmataServo example to use new pin mapping technique
+* Removed makefiles from examples (because they were not being updated)
+* Updated all examples to set current firmware version
+
+FIRMATA 2.3.6 - Jun 18, 2013 (Version included with Arduino core libraries)
+
+[core library]
+* Fixed bug introduced in 2.3.5 that broke ability to use Ethernet.
+
+FIRMATA 2.3.5 - May 21, 2013
+
+[core library]
+* Added Arduino Due to Boards.h
+* Added Teensy 3.0 to Boards.h
+* Updated unit tests to use ArduinoUnit v2.0
+* Renamed pin13strobe to strobeBlinkPin
+* Removed blinkVersion method from begin method for non-serial streams
+* Fixed memory leak in setting firmware version (Matthew Murdoch)
+* Added unit tests for a few core functions (Matthew Murdoch)
+* Added IS_PIN_SPI macro to all board definitions in Board.h (Norbert Truchsess)
+
+FIRMATA 2.3.4 - Feb 11, 2013
+
+[core library]
+* Fixed Stream implementation so Firmata can be used with Streams other than
+  Serial (Norbert Truchsess)
+
+FIRMATA 2.3.3 - Oct 6, 2012
+
+[core library]
+* Added write method to expose FirmataSerial.write
+* Added Arduino Leonardo to Boards.h
+
+[StandardFirmata]
+* Changed all instances of Serial.write to Firmata.write
+* Fixed delayMicroseconds(0) bug in readAndReportData
+
+FIRMATA 2.3.0 - 2.3.2
+
+* Removed angle from servo config
+* Changed file extensions from .pde to .ino
+* Added MEGA2560 to Boards.h
+* Added I2C pins to Boards.h
+* Modified examples to be compatible with Arduino 0022 and 1.0 or greater
+* Removed I2CFirmata example
+* Changes to StandardFirmata
+  * Added I2C support
+  * Added system reset message to reset all pins to default config on sysex reset
+
+FIRMATA 2.2 (changes prior to Firmata 2.3.0 were not well documented)
+
+* changes undocumented
+
+FIRMATA 2.1
+
+* added support for changing the sampling interval
+* added Servo support
+
+FIRMATA 2.0
+
+* changed to 8-bit port-based digital messages to mirror ports from previous 14-bit ports modeled after the standard Arduino board.
+* switched order of version message so major version is reported first
+
+FIRMATA 1.0
+
+* switched to MIDI-compatible packet format (though the message interpretation differs)
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/keywords.txt
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/keywords.txt	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/keywords.txt	(revision 224)
@@ -7,9 +7,9 @@
 #######################################
 
-Firmata	KEYWORD1
-callbackFunction	KEYWORD1
-systemResetCallbackFunction	KEYWORD1
-stringCallbackFunction	KEYWORD1
-sysexCallbackFunction	KEYWORD1
+Firmata				KEYWORD1	Firmata
+callbackFunction		KEYWORD1	callbackFunction
+systemResetCallbackFunction	KEYWORD1	systemResetCallbackFunction
+stringCallbackFunction		KEYWORD1	stringCallbackFunction
+sysexCallbackFunction		KEYWORD1	sysexCallbackFunction
 
 #######################################
@@ -17,23 +17,32 @@
 #######################################
 
-begin	KEYWORD2
-begin	KEYWORD2
-printVersion	KEYWORD2
-blinkVersion	KEYWORD2
-printFirmwareVersion	KEYWORD2
-setFirmwareVersion	KEYWORD2
+begin				KEYWORD2
+printVersion			KEYWORD2
+blinkVersion			KEYWORD2
+printFirmwareVersion		KEYWORD2
+setFirmwareVersion		KEYWORD2
 setFirmwareNameAndVersion	KEYWORD2
-available	KEYWORD2
-processInput	KEYWORD2
-sendAnalog	KEYWORD2
-sendDigital	KEYWORD2
-sendDigitalPortPair	KEYWORD2
-sendDigitalPort	KEYWORD2
-sendString	KEYWORD2
-sendString	KEYWORD2
-sendSysex	KEYWORD2
-attach	KEYWORD2
-detach	KEYWORD2
-flush	KEYWORD2
+available			KEYWORD2
+processInput			KEYWORD2
+isParsingMessage		KEYWORD2
+parse				KEYWORD2
+sendAnalog			KEYWORD2
+sendDigital			KEYWORD2
+sendDigitalPort			KEYWORD2
+sendString			KEYWORD2
+sendSysex			KEYWORD2
+getPinMode			KEYWORD2
+setPinMode			KEYWORD2
+getPinState			KEYWORD2
+setPinState			KEYWORD2
+attach				KEYWORD2
+detach				KEYWORD2
+write				KEYWORD2
+sendValueAsTwo7bitBytes	KEYWORD2
+startSysex			KEYWORD2
+endSysex			KEYWORD2
+writePort			KEYWORD2
+readPort			KEYWORD2
+disableBlinkVersion		KEYWORD2
 
 
@@ -42,21 +51,40 @@
 #######################################
 
-MAX_DATA_BYTES	LITERAL1
+FIRMATA_MAJOR_VERSION	LITERAL1
+FIRMATA_MINOR_VERSION	LITERAL1
+FIRMATA_BUGFIX_VERSION	LITERAL1
 
-DIGITAL_MESSAGE	LITERAL1
-ANALOG_MESSAGE	LITERAL1
-REPORT_ANALOG	LITERAL1
-REPORT_DIGITAL	LITERAL1
-REPORT_VERSION	LITERAL1
-SET_PIN_MODE	LITERAL1
-SYSTEM_RESET	LITERAL1
+MAX_DATA_BYTES		LITERAL1
 
-START_SYSEX	LITERAL1
-END_SYSEX	LITERAL1
+DIGITAL_MESSAGE		LITERAL1
+ANALOG_MESSAGE		LITERAL1
+REPORT_ANALOG		LITERAL1
+REPORT_DIGITAL		LITERAL1
+REPORT_VERSION		LITERAL1
+SET_PIN_MODE		LITERAL1
+SET_DIGITAL_PIN_VALUE	LITERAL1
+SYSTEM_RESET		LITERAL1
+START_SYSEX		LITERAL1
+END_SYSEX		LITERAL1
+REPORT_FIRMWARE		LITERAL1
+STRING_DATA		LITERAL1
 
-PWM	LITERAL1
+PIN_MODE_ANALOG		LITERAL1
+PIN_MODE_PWM		LITERAL1
+PIN_MODE_SERVO		LITERAL1
+PIN_MODE_SHIFT		LITERAL1
+PIN_MODE_I2C		LITERAL1
+PIN_MODE_ONEWIRE	LITERAL1
+PIN_MODE_STEPPER	LITERAL1
+PIN_MODE_ENCODER	LITERAL1
+PIN_MODE_SERIAL		LITERAL1
+PIN_MODE_PULLUP		LITERAL1
+PIN_MODE_IGNORE		LITERAL1
 
+TOTAL_PINS		LITERAL1
 TOTAL_ANALOG_PINS	LITERAL1
 TOTAL_DIGITAL_PINS	LITERAL1
-TOTAL_PORTS			LITERAL1
-ANALOG_PORT			LITERAL1
+TOTAL_PIN_MODES		LITERAL1
+TOTAL_PORTS		LITERAL1
+ANALOG_PORT		LITERAL1
+MAX_SERVOS		LITERAL1
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/library.properties
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/library.properties	(revision 136)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/library.properties	(revision 224)
@@ -1,9 +1,9 @@
 name=Firmata
-version=2.3.6
+version=2.5.2
 author=Firmata Developers
-maintainer=Firmata Developers <firmata-devel@lists.sourceforge.net>
+maintainer=https://github.com/firmata/arduino
 sentence=Enables the communication with computer apps using a standard serial protocol. For all Arduino boards.
 paragraph=The Firmata library implements the Firmata protocol for communicating with software on the host computer. This allows you to write custom firmware without having to create your own protocol and objects for the programming environment that you are using.
 category=Device Control
-url=http://firmata.org
+url=https://github.com/firmata/arduino
 architectures=*
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/readme.md
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/readme.md	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/readme.md	(revision 224)
@@ -0,0 +1,177 @@
+#Firmata
+
+[![Gitter](https://badges.gitter.im/Join Chat.svg)](https://gitter.im/firmata/arduino?utm_source=badge&utm_medium=badge&utm_campaign=pr-badge&utm_content=badge)
+
+Firmata is a protocol for communicating with microcontrollers from software on a host computer. The [protocol](https://github.com/firmata/protocol) can be implemented in firmware on any microcontroller architecture as well as software on any host computer software package. The Arduino repository described here is a Firmata library for Arduino and Arduino-compatible devices. If you would like to contribute to Firmata, please see the [Contributing](#contributing) section below.
+
+##Usage
+
+There are two main models of usage of Firmata. In one model, the author of the Arduino sketch uses the various methods provided by the Firmata library to selectively send and receive data between the Arduino device and the software running on the host computer. For example, a user can send analog data to the host using ``` Firmata.sendAnalog(analogPin, analogRead(analogPin)) ``` or send data packed in a string using ``` Firmata.sendString(stringToSend) ```. See File -> Examples -> Firmata -> AnalogFirmata & EchoString respectively for examples.
+
+The second and more common model is to load a general purpose sketch called StandardFirmata (or one of the variants such as StandardFirmataPlus or StandardFirmataEthernet depending on your needs) on the Arduino board and then use the host computer exclusively to interact with the Arduino board. StandardFirmata is located in the Arduino IDE in File -> Examples -> Firmata.
+
+##Firmata Client Libraries
+Most of the time you will be interacting with Arduino with a client library on the host computers. Several Firmata client libraries have been implemented in a variety of popular programming languages:
+
+* processing
+  * [https://github.com/firmata/processing]
+  * [http://funnel.cc]
+* python
+  * [https://github.com/MrYsLab/pymata-aio]
+  * [https://github.com/MrYsLab/PyMata]
+  * [https://github.com/tino/pyFirmata]
+  * [https://github.com/lupeke/python-firmata]
+  * [https://github.com/firmata/pyduino]
+* perl
+  * [https://github.com/ntruchsess/perl-firmata]
+  * [https://github.com/rcaputo/rx-firmata]
+* ruby
+  * [https://github.com/hardbap/firmata]
+  * [https://github.com/PlasticLizard/rufinol]
+  * [http://funnel.cc]
+* clojure
+  * [https://github.com/nakkaya/clodiuno]
+  * [https://github.com/peterschwarz/clj-firmata]
+* javascript
+  * [https://github.com/jgautier/firmata]
+  * [https://github.com/rwldrn/johnny-five]
+  * [http://breakoutjs.com]
+* java
+  * [https://github.com/kurbatov/firmata4j]
+  * [https://github.com/4ntoine/Firmata]
+  * [https://github.com/reapzor/FiloFirmata]
+* .NET
+  * [https://github.com/SolidSoils/Arduino]
+  * [http://www.imagitronics.org/projects/firmatanet/]
+* Flash/AS3
+  * [http://funnel.cc]
+  * [http://code.google.com/p/as3glue/]
+* PHP
+  * [https://bitbucket.org/ThomasWeinert/carica-firmata]
+  * [https://github.com/oasynnoum/phpmake_firmata]
+* Haskell
+  * [http://hackage.haskell.org/package/hArduino]
+* iOS
+  * [https://github.com/jacobrosenthal/iosfirmata]
+* Dart
+  * [https://github.com/nfrancois/firmata]
+* Max/MSP
+  * [http://www.maxuino.org/]
+* Elixir
+  * [https://github.com/kfatehi/firmata]
+* Modelica
+  * [https://www.wolfram.com/system-modeler/libraries/model-plug/]
+* golang
+  * [https://github.com/kraman/go-firmata]
+
+Note: The above libraries may support various versions of the Firmata protocol and therefore may not support all features of the latest Firmata spec nor all Arduino and Arduino-compatible boards. Refer to the respective projects for details.
+
+##Updating Firmata in the Arduino IDE - Arduino 1.6.4 and higher
+
+If you want to update to the latest stable version:
+
+1. Open the Arduino IDE and navigate to: `Sketch > Include Library > Manage Libraries`
+2. Filter by "Firmata" and click on the "Firmata by Firmata Developers" item in the list of results.
+3. Click the `Select version` dropdown and select the most recent version (note you can also install previous versions)
+4. Click `Install`.
+
+###Cloning Firmata
+
+If you are contributing to Firmata or otherwise need a version newer than the latest tagged release, you can clone Firmata directly to your Arduino/libraries/ directory (where 3rd party libraries are installed). This only works for Arduino 1.6.4 and higher, for older versions you need to clone into the Arduino application directory (see section below titled "Using the Source code rather than release archive"). Be sure to change the name to Firmata as follows:
+
+```bash
+$ git clone git@github.com:firmata/arduino.git ~/Documents/Arduino/libraries/Firmata
+```
+
+*Update path above if you're using Windows or Linux or changed the default Arduino directory on OS X*
+
+
+##Updating Firmata in the Arduino IDE - older versions (<= 1.6.3 or 1.0.x)
+
+Download the latest [release](https://github.com/firmata/arduino/releases/tag/2.5.2) (for Arduino 1.0.x or Arduino 1.5.6 or higher) and replace the existing Firmata folder in your Arduino application. See the instructions below for your platform.
+
+*Note that Arduino 1.5.0 - 1.5.5 are not supported. Please use Arduino 1.5.6 or higher (or Arduino 1.0.5 or 1.0.6).*
+
+###Mac OSX:
+
+The Firmata library is contained within the Arduino package.
+
+1. Navigate to the Arduino application
+2. Right click on the application icon and select `Show Package Contents`
+3. Navigate to: `/Contents/Resources/Java/libraries/` and replace the existing
+`Firmata` folder with latest [Firmata release](https://github.com/firmata/arduino/releases/tag/2.5.2) (note there is a different download
+for Arduino 1.0.x vs 1.6.x)
+4. Restart the Arduino application and the latest version of Firmata will be available.
+
+*If you are using the Java 7 version of Arduino 1.5.7 or higher, the file path
+will differ slightly: `Contents/Java/libraries/Firmata` (no Resources directory).*
+
+###Windows:
+
+1. Navigate to `c:/Program\ Files/arduino-1.x/libraries/` and replace the existing
+`Firmata` folder with the latest [Firmata release](https://github.com/firmata/arduino/releases/tag/2.5.2) (note there is a different download
+for Arduino 1.0.x vs 1.6.x).
+2. Restart the Arduino application and the latest version of Firmata will be available.
+
+*Update the path and Arduino version as necessary*
+
+###Linux:
+
+1. Navigate to `~/arduino-1.x/libraries/` and replace the existing
+`Firmata` folder with the latest [Firmata release](https://github.com/firmata/arduino/releases/tag/2.5.2) (note there is a different download
+for Arduino 1.0.x vs 1.6.x).
+2. Restart the Arduino application and the latest version of Firmata will be available.
+
+*Update the path and Arduino version as necessary*
+
+###Using the Source code rather than release archive (only for versions older than Arduino 1.6.3)
+
+*It is recommended you update to Arduino 1.6.4 or higher if possible, that way you can clone directly into the external Arduino/libraries/ directory which persists between Arduino application updates. Otherwise you will need to move your clone each time you update to a newer version of the Arduino IDE.*
+
+If you're stuck with an older version of the IDE, then follow these keep reading otherwise jump up to the "Cloning Firmata section above".
+
+Clone this repo directly into the core Arduino application libraries directory. If you are using
+Arduino 1.5.x or <= 1.6.3, the repo directory structure will not match the Arduino
+library format, however it should still compile as long as you are using Arduino 1.5.7
+or higher.
+
+You will first need to remove the existing Firmata library, then clone firmata/arduino
+into an empty Firmata directory:
+
+```bash
+$ rm -r /Applications/Arduino.app/Contents/Resources/Java/libraries/Firmata
+$ git clone git@github.com:firmata/arduino.git /Applications/Arduino.app/Contents/Resources/Java/libraries/Firmata
+```
+
+*Update paths if you're using Windows or Linux*
+
+To generate properly formatted versions of Firmata (for Arduino 1.0.x and Arduino 1.6.x), run the
+`release.sh` script.
+
+
+<a name="contributing" />
+##Contributing
+
+If you discover a bug or would like to propose a new feature, please open a new [issue](https://github.com/firmata/arduino/issues?sort=created&state=open). Due to the limited memory of standard Arduino boards we cannot add every requested feature to StandardFirmata. Requests to add new features to StandardFirmata will be evaluated by the Firmata developers. However it is still possible to add new features to other Firmata implementations (Firmata is a protocol whereas StandardFirmata is just one of many possible implementations).
+
+To contribute, fork this repository and create a new topic branch for the bug, feature or other existing issue you are addressing. Submit the pull request against the *master* branch.
+
+If you would like to contribute but don't have a specific bugfix or new feature to contribute, you can take on an existing issue, see issues labeled "pull-request-encouraged". Add a comment to the issue to express your intent to begin work and/or to get any additional information about the issue.
+
+You must thoroughly test your contributed code. In your pull request, describe tests performed to ensure that no existing code is broken and that any changes maintain backwards compatibility with the existing api. Test on multiple Arduino board variants if possible. We hope to enable some form of automated (or at least semi-automated) testing in the future, but for now any tests will need to be executed manually by the contributor and reviewers.
+
+Use [Artistic Style](http://astyle.sourceforge.net/) (astyle) to format your code. Set the following rules for the astyle formatter:
+
+```
+style = ""
+indent-spaces = 2
+indent-classes = true
+indent-switches = true
+indent-cases = true
+indent-col1-comments = true
+pad-oper = true
+pad-header = true
+keep-one-line-statements = true
+```
+
+If you happen to use Sublime Text, [this astyle plugin](https://github.com/timonwong/SublimeAStyleFormatter) is helpful. Set the above rules in the user settings file.
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/release.sh
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/release.sh	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/release.sh	(revision 224)
@@ -0,0 +1,35 @@
+#!/bin/sh
+
+# use this script to package Firmata for distribution
+
+# package for Arduino 1.0.x
+mkdir -p temp/Firmata
+cp -r examples temp/Firmata
+cp -r extras temp/Firmata
+cp -r utility temp/Firmata
+cp Boards.h temp/Firmata
+cp Firmata.cpp temp/Firmata
+cp Firmata.h temp/Firmata
+cp keywords.txt temp/Firmata
+cp readme.md temp/Firmata
+cd temp
+find . -name "*.DS_Store" -type f -delete
+zip -r Firmata.zip ./Firmata/
+cd ..
+mv ./temp/Firmata.zip Firmata-2.5.2.zip
+
+#package for Arduino 1.6.x
+cp library.properties temp/Firmata
+cd temp/Firmata
+mv readme.md ./extras/
+mkdir src
+mv Boards.h ./src/
+mv Firmata.cpp ./src/
+mv Firmata.h ./src/
+mv utility ./src/
+cd ..
+find . -name "*.DS_Store" -type f -delete
+zip -r Firmata.zip ./Firmata/
+cd ..
+mv ./temp/Firmata.zip Arduino-1.6.x-Firmata-2.5.2.zip
+rm -r ./temp
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/firmata_test/firmata_test.ino
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/firmata_test/firmata_test.ino	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/firmata_test/firmata_test.ino	(revision 224)
@@ -0,0 +1,136 @@
+/*
+ * To run this test suite, you must first install the ArduinoUnit library
+ * to your Arduino/libraries/ directory.
+ * You can get ArduinoUnit here: https://github.com/mmurdoch/arduinounit
+ * Download version 2.0 or greater or install it via the Arduino library manager.
+ */
+
+#include <ArduinoUnit.h>
+#include <Firmata.h>
+
+void setup()
+{
+  Serial.begin(9600);
+}
+
+void loop()
+{
+  Test::run();
+}
+
+test(beginPrintsVersion)
+{
+  FakeStream stream;
+
+  Firmata.begin(stream);
+
+  char expected[] = {
+    REPORT_VERSION,
+    FIRMATA_PROTOCOL_MAJOR_VERSION,
+    FIRMATA_PROTOCOL_MINOR_VERSION,
+    0
+  };
+  assertEqual(expected, stream.bytesWritten());
+}
+
+void processMessage(const byte *message, size_t length)
+{
+  FakeStream stream;
+  Firmata.begin(stream);
+
+  for (size_t i = 0; i < length; i++) {
+    stream.nextByte(message[i]);
+    Firmata.processInput();
+  }
+}
+
+byte _digitalPort;
+int _digitalPortValue;
+void writeToDigitalPort(byte port, int value)
+{
+  _digitalPort = port;
+  _digitalPortValue = value;
+}
+
+void setupDigitalPort()
+{
+  _digitalPort = 0;
+  _digitalPortValue = 0;
+}
+
+test(processWriteDigital_0)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0, 0 };
+  processMessage(message, 3);
+
+  assertEqual(0, _digitalPortValue);
+}
+
+test(processWriteDigital_127)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 127, 0 };
+  processMessage(message, 3);
+
+  assertEqual(127, _digitalPortValue);
+}
+
+test(processWriteDigital_128)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0, 1 };
+  processMessage(message, 3);
+
+  assertEqual(128, _digitalPortValue);
+}
+
+test(processWriteLargestDigitalValue)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0x7F, 0x7F };
+  processMessage(message, 3);
+
+  // Maximum of 14 bits can be set (B0011111111111111)
+  assertEqual(0x3FFF, _digitalPortValue);
+}
+
+test(defaultDigitalWritePortIsZero)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE, 0, 0 };
+  processMessage(message, 3);
+
+  assertEqual(0, _digitalPort);
+}
+
+test(specifiedDigitalWritePort)
+{
+  setupDigitalPort();
+  Firmata.attach(DIGITAL_MESSAGE, writeToDigitalPort);
+
+  byte message[] = { DIGITAL_MESSAGE + 1, 0, 0 };
+  processMessage(message, 3);
+
+  assertEqual(1, _digitalPort);
+}
+
+test(setFirmwareVersionDoesNotLeakMemory)
+{
+  Firmata.setFirmwareVersion(1, 0);
+  int initialMemory = freeMemory();
+
+  Firmata.setFirmwareVersion(1, 0);
+
+  assertEqual(0, initialMemory - freeMemory());
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/readme.md
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/readme.md	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/test/readme.md	(revision 224)
@@ -0,0 +1,13 @@
+#Testing Firmata
+
+Tests tests are written using the [ArduinoUnit](https://github.com/mmurdoch/arduinounit) library (version 2.0).
+
+Follow the instructions in the [ArduinoUnit readme](https://github.com/mmurdoch/arduinounit/blob/master/readme.md) to install the library.
+
+Compile and upload the test sketch as you would any other sketch. Then open the
+Serial Monitor to view the test results.
+
+If you make changes to Firmata.cpp, run the tests in /test/ to ensure
+that your changes have not produced any unexpected errors.
+
+You should also perform manual tests against actual hardware.
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.cpp	(revision 224)
@@ -0,0 +1,3 @@
+/*
+ * Implementation is in BLEStream.h to avoid linker issues.
+ */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/BLEStream.h	(revision 224)
@@ -0,0 +1,243 @@
+/*
+  BLEStream.h
+
+  Based on BLESerial.cpp by Voita Molda
+  https://github.com/sandeepmistry/arduino-BLEPeripheral/blob/master/examples/serial/BLESerial.h
+
+  Last updated April 04th, 2016
+ */
+
+#ifndef _BLE_STREAM_H_
+#define _BLE_STREAM_H_
+
+#include <Arduino.h>
+#if defined(_VARIANT_ARDUINO_101_X_)
+#include <CurieBLE.h>
+#define _MAX_ATTR_DATA_LEN_ BLE_MAX_ATTR_DATA_LEN
+#else
+#include <BLEPeripheral.h>
+#define _MAX_ATTR_DATA_LEN_ BLE_ATTRIBUTE_MAX_VALUE_LENGTH
+#endif
+
+#define BLESTREAM_TXBUFFER_FLUSH_INTERVAL 80
+#define BLESTREAM_MIN_FLUSH_INTERVAL 8 // minimum interval for flushing the TX buffer
+
+// #define BLE_SERIAL_DEBUG
+
+class BLEStream : public BLEPeripheral, public Stream
+{
+  public:
+    BLEStream(unsigned char req = 0, unsigned char rdy = 0, unsigned char rst = 0);
+
+    void begin(...);
+    bool poll();
+    void end();
+    void setFlushInterval(int);
+
+    virtual int available(void);
+    virtual int peek(void);
+    virtual int read(void);
+    virtual void flush(void);
+    virtual size_t write(uint8_t byte);
+    using Print::write;
+    virtual operator bool();
+
+  private:
+    bool _connected;
+    unsigned long _flushed;
+    int _flushInterval;
+    static BLEStream* _instance;
+
+    size_t _rxHead;
+    size_t _rxTail;
+    size_t _rxCount() const;
+    unsigned char _rxBuffer[256];
+    size_t _txCount;
+    unsigned char _txBuffer[_MAX_ATTR_DATA_LEN_];
+
+    BLEService _uartService = BLEService("6E400001-B5A3-F393-E0A9-E50E24DCCA9E");
+    BLEDescriptor _uartNameDescriptor = BLEDescriptor("2901", "UART");
+    BLECharacteristic _rxCharacteristic = BLECharacteristic("6E400002-B5A3-F393-E0A9-E50E24DCCA9E", BLEWriteWithoutResponse, _MAX_ATTR_DATA_LEN_);
+    BLEDescriptor _rxNameDescriptor = BLEDescriptor("2901", "RX - Receive Data (Write)");
+    BLECharacteristic _txCharacteristic = BLECharacteristic("6E400003-B5A3-F393-E0A9-E50E24DCCA9E", BLENotify, _MAX_ATTR_DATA_LEN_);
+    BLEDescriptor _txNameDescriptor = BLEDescriptor("2901", "TX - Transfer Data (Notify)");
+
+    void _received(const unsigned char* data, size_t size);
+    static void _received(BLECentral& /*central*/, BLECharacteristic& rxCharacteristic);
+};
+
+
+/*
+ * BLEStream.cpp
+ * Copied here as a hack to avoid having to install the BLEPeripheral libarary even if it's
+ * not needed.
+ */
+
+BLEStream* BLEStream::_instance = NULL;
+
+BLEStream::BLEStream(unsigned char req, unsigned char rdy, unsigned char rst) :
+#if defined(_VARIANT_ARDUINO_101_X_)
+  BLEPeripheral()
+#else
+  BLEPeripheral(req, rdy, rst)
+#endif
+{
+  this->_txCount = 0;
+  this->_rxHead = this->_rxTail = 0;
+  this->_flushed = 0;
+  this->_flushInterval = BLESTREAM_TXBUFFER_FLUSH_INTERVAL;
+  BLEStream::_instance = this;
+
+  addAttribute(this->_uartService);
+  addAttribute(this->_uartNameDescriptor);
+  setAdvertisedServiceUuid(this->_uartService.uuid());
+  addAttribute(this->_rxCharacteristic);
+  addAttribute(this->_rxNameDescriptor);
+  this->_rxCharacteristic.setEventHandler(BLEWritten, BLEStream::_received);
+  addAttribute(this->_txCharacteristic);
+  addAttribute(this->_txNameDescriptor);
+}
+
+void BLEStream::begin(...)
+{
+  BLEPeripheral::begin();
+#ifdef BLE_SERIAL_DEBUG
+  Serial.println(F("BLEStream::begin()"));
+#endif
+}
+
+bool BLEStream::poll()
+{
+  // BLEPeripheral::poll is called each time connected() is called
+  this->_connected = BLEPeripheral::connected();
+  if (millis() > this->_flushed + this->_flushInterval) {
+    flush();
+  }
+  return this->_connected;
+}
+
+void BLEStream::end()
+{
+  this->_rxCharacteristic.setEventHandler(BLEWritten, NULL);
+  this->_rxHead = this->_rxTail = 0;
+  flush();
+  BLEPeripheral::disconnect();
+}
+
+int BLEStream::available(void)
+{
+// BLEPeripheral::poll only calls delay(1) in CurieBLE so skipping it here to avoid the delay
+#ifndef _VARIANT_ARDUINO_101_X_
+  // TODO Need to do more testing to determine if all of these calls to BLEPeripheral::poll are
+  // actually necessary. Seems to run fine without them, but only minimal testing so far.
+  BLEPeripheral::poll();
+#endif
+  int retval = (this->_rxHead - this->_rxTail + sizeof(this->_rxBuffer)) % sizeof(this->_rxBuffer);
+#ifdef BLE_SERIAL_DEBUG
+  if (retval > 0) {
+    Serial.print(F("BLEStream::available() = "));
+    Serial.println(retval);
+  }
+#endif
+  return retval;
+}
+
+int BLEStream::peek(void)
+{
+#ifndef _VARIANT_ARDUINO_101_X_
+  BLEPeripheral::poll();
+#endif
+  if (this->_rxTail == this->_rxHead) return -1;
+  uint8_t byte = this->_rxBuffer[this->_rxTail];
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::peek() = 0x"));
+  Serial.println(byte, HEX);
+#endif
+  return byte;
+}
+
+int BLEStream::read(void)
+{
+#ifndef _VARIANT_ARDUINO_101_X_
+  BLEPeripheral::poll();
+#endif
+  if (this->_rxTail == this->_rxHead) return -1;
+  this->_rxTail = (this->_rxTail + 1) % sizeof(this->_rxBuffer);
+  uint8_t byte = this->_rxBuffer[this->_rxTail];
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::read() = 0x"));
+  Serial.println(byte, HEX);
+#endif
+  return byte;
+}
+
+void BLEStream::flush(void)
+{
+  if (this->_txCount == 0) return;
+#ifndef _VARIANT_ARDUINO_101_X_
+  // ensure there are available packets before sending
+  while(!this->_txCharacteristic.canNotify()) {
+    BLEPeripheral::poll();
+  }
+#endif
+  this->_txCharacteristic.setValue(this->_txBuffer, this->_txCount);
+  this->_flushed = millis();
+  this->_txCount = 0;
+#ifdef BLE_SERIAL_DEBUG
+  Serial.println(F("BLEStream::flush()"));
+#endif
+}
+
+size_t BLEStream::write(uint8_t byte)
+{
+#ifndef _VARIANT_ARDUINO_101_X_
+  BLEPeripheral::poll();
+#endif
+  if (this->_txCharacteristic.subscribed() == false) return 0;
+  this->_txBuffer[this->_txCount++] = byte;
+  if (this->_txCount == sizeof(this->_txBuffer)) flush();
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::write( 0x"));
+  Serial.print(byte, HEX);
+  Serial.println(F(") = 1"));
+#endif
+  return 1;
+}
+
+BLEStream::operator bool()
+{
+  bool retval = this->_connected = BLEPeripheral::connected();
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::operator bool() = "));
+  Serial.println(retval);
+#endif
+  return retval;
+}
+
+void BLEStream::setFlushInterval(int interval)
+{
+  if (interval > BLESTREAM_MIN_FLUSH_INTERVAL) {
+    this->_flushInterval = interval;
+  }
+}
+
+void BLEStream::_received(const unsigned char* data, size_t size)
+{
+  for (size_t i = 0; i < size; i++) {
+    this->_rxHead = (this->_rxHead + 1) % sizeof(this->_rxBuffer);
+    this->_rxBuffer[this->_rxHead] = data[i];
+  }
+#ifdef BLE_SERIAL_DEBUG
+  Serial.print(F("BLEStream::received("));
+  for (int i = 0; i < size; i++) Serial.print(data[i], HEX);
+  Serial.println(F(")"));
+#endif
+}
+
+void BLEStream::_received(BLECentral& /*central*/, BLECharacteristic& rxCharacteristic)
+{
+  BLEStream::_instance->_received(rxCharacteristic.value(), rxCharacteristic.valueLength());
+}
+
+
+#endif // _BLE_STREAM_H_
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.cpp	(revision 224)
@@ -0,0 +1,114 @@
+/*
+  EthernetClientStream.cpp
+  An Arduino-Stream that wraps an instance of Client reconnecting to
+  the remote-ip in a transparent way. A disconnected client may be
+  recognized by the returnvalues -1 from calls to peek or read and
+  a 0 from calls to write.
+
+  Copyright (C) 2013 Norbert Truchsess. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  formatted using the GNU C formatting and indenting
+ */
+
+#include "EthernetClientStream.h"
+#include <Arduino.h>
+
+//#define SERIAL_DEBUG
+#include "firmataDebug.h"
+
+#define MILLIS_RECONNECT 5000
+
+EthernetClientStream::EthernetClientStream(Client &client, IPAddress localip, IPAddress ip, const char* host, uint16_t port)
+: client(client),
+  localip(localip),
+  ip(ip),
+  host(host),
+  port(port),
+  connected(false)
+{
+}
+
+int
+EthernetClientStream::available()
+{
+  return maintain() ? client.available() : 0;
+}
+
+int
+EthernetClientStream::read()
+{
+  return maintain() ? client.read() : -1;
+}
+
+int
+EthernetClientStream::peek()
+{
+  return maintain() ? client.peek() : -1;
+}
+
+void EthernetClientStream::flush()
+{
+  if (maintain())
+    client.flush();
+}
+
+size_t
+EthernetClientStream::write(uint8_t c)
+{
+  return maintain() ? client.write(c) : 0;
+}
+
+void
+EthernetClientStream::maintain(IPAddress localip)
+{
+// temporary hack to Firmata to compile for Intel Galileo
+// the issue is documented here: https://github.com/firmata/arduino/issues/218
+#if !defined(ARDUINO_LINUX)
+  // ensure the local IP is updated in the case that it is changed by the DHCP server
+  if (this->localip != localip)
+    {
+      this->localip = localip;
+      if (connected)
+        stop();
+    }
+#endif
+}
+
+void
+EthernetClientStream::stop()
+{
+  client.stop();
+  connected = false;
+  time_connect = millis();
+}
+
+bool
+EthernetClientStream::maintain()
+{
+  if (client && client.connected())
+    return true;
+
+  if (connected)
+    {
+      stop();
+    }
+  // if the client is disconnected, attempt to reconnect every 5 seconds
+  else if (millis()-time_connect >= MILLIS_RECONNECT)
+    {
+      connected = host ? client.connect(host, port) : client.connect(ip, port);
+      if (!connected) {
+        time_connect = millis();
+        DEBUG_PRINTLN("connection failed. attempting to reconnect...");
+      } else {
+        DEBUG_PRINTLN("connected");
+      }
+    }
+  return connected;
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/EthernetClientStream.h	(revision 224)
@@ -0,0 +1,52 @@
+/*
+  EthernetClientStream.h
+  An Arduino-Stream that wraps an instance of Client reconnecting to
+  the remote-ip in a transparent way. A disconnected client may be
+  recognized by the returnvalues -1 from calls to peek or read and
+  a 0 from calls to write.
+
+  Copyright (C) 2013 Norbert Truchsess. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  formatted using the GNU C formatting and indenting
+ */
+
+#ifndef ETHERNETCLIENTSTREAM_H
+#define ETHERNETCLIENTSTREAM_H
+
+#include <inttypes.h>
+#include <stdio.h>
+#include <Stream.h>
+#include <Client.h>
+#include <IPAddress.h>
+
+class EthernetClientStream : public Stream
+{
+public:
+  EthernetClientStream(Client &client, IPAddress localip, IPAddress ip, const char* host, uint16_t port);
+  int available();
+  int read();
+  int peek();
+  void flush();
+  size_t write(uint8_t);
+  void maintain(IPAddress localip);
+
+private:
+  Client &client;
+  IPAddress localip;
+  IPAddress ip;
+  const char* host;
+  uint16_t port;
+  bool connected;
+  uint32_t time_connect;
+  bool maintain();
+  void stop();
+};
+
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/FirmataFeature.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/FirmataFeature.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/FirmataFeature.h	(revision 224)
@@ -0,0 +1,38 @@
+/*
+  FirmataFeature.h
+  Copyright (C) 2006-2008 Hans-Christoph Steiner.  All rights reserved.
+  Copyright (C) 2010-2011 Paul Stoffregen.  All rights reserved.
+  Copyright (C) 2009 Shigeru Kobayashi.  All rights reserved.
+  Copyright (C) 2013 Norbert Truchsess. All rights reserved.
+  Copyright (C) 2009-2016 Jeff Hoefs.  All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  Interface for Firmata feature classes.
+
+  This version of FirmataFeature.h differs from the ConfigurableFirmata
+  version in the following ways:
+
+  - Imports Firmata.h rather than ConfigurableFirmata.h
+
+  See file LICENSE.txt for further informations on licensing terms.
+*/
+
+#ifndef FirmataFeature_h
+#define FirmataFeature_h
+
+#include <Firmata.h>
+
+class FirmataFeature
+{
+  public:
+    virtual void handleCapability(byte pin) = 0;
+    virtual boolean handlePinMode(byte pin, int mode) = 0;
+    virtual boolean handleSysex(byte command, byte argc, byte* argv) = 0;
+    virtual void reset() = 0;
+};
+
+#endif
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.cpp	(revision 224)
@@ -0,0 +1,328 @@
+/*
+  SerialFirmata.cpp
+  Copyright (C) 2016 Jeff Hoefs. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  This version of SerialFirmata.cpp differs from the ConfigurableFirmata
+  version in the following ways:
+
+  - handlePinMode calls Firmata::setPinMode
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+#include "SerialFirmata.h"
+
+SerialFirmata::SerialFirmata()
+{
+  swSerial0 = NULL;
+  swSerial1 = NULL;
+  swSerial2 = NULL;
+  swSerial3 = NULL;
+
+  serialIndex = -1;
+}
+
+boolean SerialFirmata::handlePinMode(byte pin, int mode)
+{
+  // used for both HW and SW serial
+  if (mode == PIN_MODE_SERIAL) {
+    Firmata.setPinMode(pin, PIN_MODE_SERIAL);
+    return true;
+  }
+  return false;
+}
+
+void SerialFirmata::handleCapability(byte pin)
+{
+  if (IS_PIN_SERIAL(pin)) {
+    Firmata.write(PIN_MODE_SERIAL);
+    Firmata.write(getSerialPinType(pin));
+  }
+}
+
+boolean SerialFirmata::handleSysex(byte command, byte argc, byte *argv)
+{
+  if (command == SERIAL_MESSAGE) {
+
+    Stream *serialPort;
+    byte mode = argv[0] & SERIAL_MODE_MASK;
+    byte portId = argv[0] & SERIAL_PORT_ID_MASK;
+
+    switch (mode) {
+      case SERIAL_CONFIG:
+        {
+          long baud = (long)argv[1] | ((long)argv[2] << 7) | ((long)argv[3] << 14);
+          serial_pins pins;
+
+          if (portId < 8) {
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              pins = getSerialPinNumbers(portId);
+              if (pins.rx != 0 && pins.tx != 0) {
+                Firmata.setPinMode(pins.rx, PIN_MODE_SERIAL);
+                Firmata.setPinMode(pins.tx, PIN_MODE_SERIAL);
+                // Fixes an issue where some serial devices would not work properly with Arduino Due
+                // because all Arduino pins are set to OUTPUT by default in StandardFirmata.
+                pinMode(pins.rx, INPUT);
+              }
+              ((HardwareSerial*)serialPort)->begin(baud);
+            }
+          } else {
+#if defined(SoftwareSerial_h)
+            byte swTxPin, swRxPin;
+            if (argc > 4) {
+              swRxPin = argv[4];
+              swTxPin = argv[5];
+            } else {
+              // RX and TX pins must be specified when using SW serial
+              Firmata.sendString("Specify serial RX and TX pins");
+              return false;
+            }
+            switch (portId) {
+              case SW_SERIAL0:
+                if (swSerial0 == NULL) {
+                  swSerial0 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL1:
+                if (swSerial1 == NULL) {
+                  swSerial1 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL2:
+                if (swSerial2 == NULL) {
+                  swSerial2 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+              case SW_SERIAL3:
+                if (swSerial3 == NULL) {
+                  swSerial3 = new SoftwareSerial(swRxPin, swTxPin);
+                }
+                break;
+            }
+            serialPort = getPortFromId(portId);
+            if (serialPort != NULL) {
+              Firmata.setPinMode(swRxPin, PIN_MODE_SERIAL);
+              Firmata.setPinMode(swTxPin, PIN_MODE_SERIAL);
+              ((SoftwareSerial*)serialPort)->begin(baud);
+            }
+#endif
+          }
+          break; // SERIAL_CONFIG
+        }
+      case SERIAL_WRITE:
+        {
+          byte data;
+          serialPort = getPortFromId(portId);
+          if (serialPort == NULL) {
+            break;
+          }
+          for (byte i = 1; i < argc; i += 2) {
+            data = argv[i] + (argv[i + 1] << 7);
+            serialPort->write(data);
+          }
+          break; // SERIAL_WRITE
+        }
+      case SERIAL_READ:
+        if (argv[1] == SERIAL_READ_CONTINUOUSLY) {
+          if (serialIndex + 1 >= MAX_SERIAL_PORTS) {
+            break;
+          }
+
+          if (argc > 2) {
+            // maximum number of bytes to read from buffer per iteration of loop()
+            serialBytesToRead[portId] = (int)argv[2] | ((int)argv[3] << 7);
+          } else {
+            // read all available bytes per iteration of loop()
+            serialBytesToRead[portId] = 0;
+          }
+          serialIndex++;
+          reportSerial[serialIndex] = portId;
+        } else if (argv[1] == SERIAL_STOP_READING) {
+          byte serialIndexToSkip = 0;
+          if (serialIndex <= 0) {
+            serialIndex = -1;
+          } else {
+            for (byte i = 0; i < serialIndex + 1; i++) {
+              if (reportSerial[i] == portId) {
+                serialIndexToSkip = i;
+                break;
+              }
+            }
+            // shift elements over to fill space left by removed element
+            for (byte i = serialIndexToSkip; i < serialIndex + 1; i++) {
+              if (i < MAX_SERIAL_PORTS) {
+                reportSerial[i] = reportSerial[i + 1];
+              }
+            }
+            serialIndex--;
+          }
+        }
+        break; // SERIAL_READ
+      case SERIAL_CLOSE:
+        serialPort = getPortFromId(portId);
+        if (serialPort != NULL) {
+          if (portId < 8) {
+            ((HardwareSerial*)serialPort)->end();
+          } else {
+#if defined(SoftwareSerial_h)
+            ((SoftwareSerial*)serialPort)->end();
+            if (serialPort != NULL) {
+              free(serialPort);
+              serialPort = NULL;
+            }
+#endif
+          }
+        }
+        break; // SERIAL_CLOSE
+      case SERIAL_FLUSH:
+        serialPort = getPortFromId(portId);
+        if (serialPort != NULL) {
+          getPortFromId(portId)->flush();
+        }
+        break; // SERIAL_FLUSH
+#if defined(SoftwareSerial_h)
+      case SERIAL_LISTEN:
+        // can only call listen() on software serial ports
+        if (portId > 7) {
+          serialPort = getPortFromId(portId);
+          if (serialPort != NULL) {
+            ((SoftwareSerial*)serialPort)->listen();
+          }
+        }
+        break; // SERIAL_LISTEN
+#endif
+    } // end switch
+    return true;
+  }
+  return false;
+}
+
+void SerialFirmata::update()
+{
+  checkSerial();
+}
+
+void SerialFirmata::reset()
+{
+#if defined(SoftwareSerial_h)
+  Stream *serialPort;
+  // free memory allocated for SoftwareSerial ports
+  for (byte i = SW_SERIAL0; i < SW_SERIAL3 + 1; i++) {
+    serialPort = getPortFromId(i);
+    if (serialPort != NULL) {
+      free(serialPort);
+      serialPort = NULL;
+    }
+  }
+#endif
+
+  serialIndex = -1;
+  for (byte i = 0; i < SERIAL_READ_ARR_LEN; i++) {
+    serialBytesToRead[i] = 0;
+  }
+}
+
+// get a pointer to the serial port associated with the specified port id
+Stream* SerialFirmata::getPortFromId(byte portId)
+{
+  switch (portId) {
+    case HW_SERIAL0:
+      // block use of Serial (typically pins 0 and 1) until ability to reclaim Serial is implemented
+      //return &Serial;
+      return NULL;
+#if defined(PIN_SERIAL1_RX)
+    case HW_SERIAL1:
+      return &Serial1;
+#endif
+#if defined(PIN_SERIAL2_RX)
+    case HW_SERIAL2:
+      return &Serial2;
+#endif
+#if defined(PIN_SERIAL3_RX)
+    case HW_SERIAL3:
+      return &Serial3;
+#endif
+#if defined(SoftwareSerial_h)
+    case SW_SERIAL0:
+      if (swSerial0 != NULL) {
+        // instances of SoftwareSerial are already pointers so simply return the instance
+        return swSerial0;
+      }
+      break;
+    case SW_SERIAL1:
+      if (swSerial1 != NULL) {
+        return swSerial1;
+      }
+      break;
+    case SW_SERIAL2:
+      if (swSerial2 != NULL) {
+        return swSerial2;
+      }
+      break;
+    case SW_SERIAL3:
+      if (swSerial3 != NULL) {
+        return swSerial3;
+      }
+      break;
+#endif
+  }
+  return NULL;
+}
+
+// Check serial ports that have READ_CONTINUOUS mode set and relay any data
+// for each port to the device attached to that port.
+void SerialFirmata::checkSerial()
+{
+  byte portId, serialData;
+  int bytesToRead = 0;
+  int numBytesToRead = 0;
+  Stream* serialPort;
+
+  if (serialIndex > -1) {
+
+    // loop through all reporting (READ_CONTINUOUS) serial ports
+    for (byte i = 0; i < serialIndex + 1; i++) {
+      portId = reportSerial[i];
+      bytesToRead = serialBytesToRead[portId];
+      serialPort = getPortFromId(portId);
+      if (serialPort == NULL) {
+        continue;
+      }
+#if defined(SoftwareSerial_h)
+      // only the SoftwareSerial port that is "listening" can read data
+      if (portId > 7 && !((SoftwareSerial*)serialPort)->isListening()) {
+        continue;
+      }
+#endif
+      if (serialPort->available() > 0) {
+        Firmata.write(START_SYSEX);
+        Firmata.write(SERIAL_MESSAGE);
+        Firmata.write(SERIAL_REPLY | portId);
+
+        if (bytesToRead == 0 || (serialPort->available() <= bytesToRead)) {
+          numBytesToRead = serialPort->available();
+        } else {
+          numBytesToRead = bytesToRead;
+        }
+
+        // relay serial data to the serial device
+        while (numBytesToRead > 0) {
+          serialData = serialPort->read();
+          Firmata.write(serialData & 0x7F);
+          Firmata.write((serialData >> 7) & 0x7F);
+          numBytesToRead--;
+        }
+        Firmata.write(END_SYSEX);
+      }
+
+    }
+  }
+}
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/SerialFirmata.h	(revision 224)
@@ -0,0 +1,167 @@
+/*
+  SerialFirmata.h
+  Copyright (C) 2016 Jeff Hoefs. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+
+  This version of SerialFirmata.h differs from the ConfigurableFirmata
+  version in the following ways:
+
+  - Defines FIRMATA_SERIAL_FEATURE (could add to Configurable version as well)
+  - Imports Firmata.h rather than ConfigurableFirmata.h
+
+  Last updated by Jeff Hoefs: January 10th, 2016
+*/
+
+#ifndef SerialFirmata_h
+#define SerialFirmata_h
+
+#include <Firmata.h>
+#include "FirmataFeature.h"
+// SoftwareSerial is currently only supported for AVR-based boards and the Arduino 101
+// The third condition checks if the IDE is in the 1.0.x series, if so, include SoftwareSerial
+// since it should be available to all boards in that IDE.
+#if defined(ARDUINO_ARCH_AVR) || defined(ARDUINO_ARCH_ARC32) || (ARDUINO >= 100 && ARDUINO < 10500)
+#include <SoftwareSerial.h>
+#endif
+
+#define FIRMATA_SERIAL_FEATURE
+
+// Serial port Ids
+#define HW_SERIAL0                  0x00
+#define HW_SERIAL1                  0x01
+#define HW_SERIAL2                  0x02
+#define HW_SERIAL3                  0x03
+// extensible up to 0x07
+
+#define SW_SERIAL0                  0x08
+#define SW_SERIAL1                  0x09
+#define SW_SERIAL2                  0x0A
+#define SW_SERIAL3                  0x0B
+// extensible up to 0x0F
+
+#define SERIAL_PORT_ID_MASK         0x0F
+#define MAX_SERIAL_PORTS            8
+#define SERIAL_READ_ARR_LEN         12
+
+// map configuration query response resolution value to serial pin type
+#define RES_RX1                     0x02
+#define RES_TX1                     0x03
+#define RES_RX2                     0x04
+#define RES_TX2                     0x05
+#define RES_RX3                     0x06
+#define RES_TX3                     0x07
+
+// Serial command bytes
+#define SERIAL_CONFIG               0x10
+#define SERIAL_WRITE                0x20
+#define SERIAL_READ                 0x30
+#define SERIAL_REPLY                0x40
+#define SERIAL_CLOSE                0x50
+#define SERIAL_FLUSH                0x60
+#define SERIAL_LISTEN               0x70
+
+// Serial read modes
+#define SERIAL_READ_CONTINUOUSLY    0x00
+#define SERIAL_STOP_READING         0x01
+#define SERIAL_MODE_MASK            0xF0
+
+namespace {
+
+  struct serial_pins {
+    uint8_t rx;
+    uint8_t tx;
+  };
+
+  /*
+   * Get the serial serial pin type (RX1, TX1, RX2, TX2, etc) for the specified pin.
+   */
+  inline uint8_t getSerialPinType(uint8_t pin) {
+  #if defined(PIN_SERIAL_RX)
+    // TODO when use of HW_SERIAL0 is enabled
+  #endif
+  #if defined(PIN_SERIAL1_RX)
+    if (pin == PIN_SERIAL1_RX) return RES_RX1;
+    if (pin == PIN_SERIAL1_TX) return RES_TX1;
+  #endif
+  #if defined(PIN_SERIAL2_RX)
+    if (pin == PIN_SERIAL2_RX) return RES_RX2;
+    if (pin == PIN_SERIAL2_TX) return RES_TX2;
+  #endif
+  #if defined(PIN_SERIAL3_RX)
+    if (pin == PIN_SERIAL3_RX) return RES_RX3;
+    if (pin == PIN_SERIAL3_TX) return RES_TX3;
+  #endif
+    return 0;
+  }
+
+  /*
+   * Get the RX and TX pins numbers for the specified HW serial port.
+   */
+  inline serial_pins getSerialPinNumbers(uint8_t portId) {
+    serial_pins pins;
+    switch (portId) {
+  #if defined(PIN_SERIAL_RX)
+        // case HW_SERIAL0:
+        //   // TODO when use of HW_SERIAL0 is enabled
+        //   break;
+  #endif
+  #if defined(PIN_SERIAL1_RX)
+      case HW_SERIAL1:
+        pins.rx = PIN_SERIAL1_RX;
+        pins.tx = PIN_SERIAL1_TX;
+        break;
+  #endif
+  #if defined(PIN_SERIAL2_RX)
+      case HW_SERIAL2:
+        pins.rx = PIN_SERIAL2_RX;
+        pins.tx = PIN_SERIAL2_TX;
+        break;
+  #endif
+  #if defined(PIN_SERIAL3_RX)
+      case HW_SERIAL3:
+        pins.rx = PIN_SERIAL3_RX;
+        pins.tx = PIN_SERIAL3_TX;
+        break;
+  #endif
+      default:
+        pins.rx = 0;
+        pins.tx = 0;
+    }
+    return pins;
+  }
+
+} // end namespace
+
+
+class SerialFirmata: public FirmataFeature
+{
+  public:
+    SerialFirmata();
+    boolean handlePinMode(byte pin, int mode);
+    void handleCapability(byte pin);
+    boolean handleSysex(byte command, byte argc, byte* argv);
+    void update();
+    void reset();
+    void checkSerial();
+
+  private:
+    byte reportSerial[MAX_SERIAL_PORTS];
+    int serialBytesToRead[SERIAL_READ_ARR_LEN];
+    signed char serialIndex;
+
+    Stream *swSerial0;
+    Stream *swSerial1;
+    Stream *swSerial2;
+    Stream *swSerial3;
+
+    Stream* getPortFromId(byte portId);
+
+};
+
+#endif /* SerialFirmata_h */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.cpp	(revision 224)
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in WiFi101Stream.h to avoid linker issues. Legacy WiFi and modern WiFi101 both define WiFiClass which
+ * will cause linker errors whenever Firmata.h is included.
+ */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFi101Stream.h	(revision 224)
@@ -0,0 +1,259 @@
+/*
+  WiFi101Stream.h
+  An Arduino Stream that wraps an instance of a WiFi101 server. For use
+  with Arduino WiFi 101 shield, Arduino MKR1000 and other boards and
+  shields that are compatible with the Arduino WiFi101 library.
+
+  Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+ */
+
+#ifndef WIFI101_STREAM_H
+#define WIFI101_STREAM_H
+
+#include <inttypes.h>
+#include <Stream.h>
+#include <WiFi101.h>
+
+
+class WiFi101Stream : public Stream
+{
+private:
+  WiFiServer _server = WiFiServer(23);
+  WiFiClient _client;
+
+  //configuration members
+  IPAddress _local_ip;
+  uint16_t _port = 0;
+  uint8_t _key_idx = 0;               //WEP
+  const char *_key = nullptr;         //WEP
+  const char *_passphrase = nullptr;  //WPA
+  char *_ssid = nullptr;
+
+  inline int connect_client()
+  {
+    if( !( _client && _client.connected() ) )
+    {
+      WiFiClient newClient = _server.available();
+      if( !newClient )
+      {
+        return 0;
+      }
+
+      _client = newClient;
+    }
+    return 1;
+  }
+
+  inline bool is_ready()
+  {
+    uint8_t status = WiFi.status();
+    return !( status == WL_NO_SHIELD || status == WL_CONNECTED );
+  }
+
+public:
+  WiFi101Stream() {};
+
+  // allows another way to configure a static IP before begin is called
+  inline void config(IPAddress local_ip)
+  {
+    _local_ip = local_ip;
+    WiFi.config( local_ip );
+  }
+
+  // get DCHP IP
+  inline IPAddress localIP()
+  {
+    return WiFi.localIP();
+  }
+
+  inline bool maintain()
+  {
+    if( connect_client() ) return true;
+
+    stop();
+    int result = 0;
+    if( WiFi.status() != WL_CONNECTED )
+    {
+      if( _local_ip )
+      {
+        WiFi.config( _local_ip );
+      }
+
+      if( _passphrase )
+      {
+        result = WiFi.begin( _ssid, _passphrase);
+      }
+      else if( _key_idx && _key )
+      {
+        result = WiFi.begin( _ssid, _key_idx, _key );
+      }
+      else
+      {
+        result = WiFi.begin( _ssid );
+      }
+    }
+    if( result == 0 ) return false;
+
+    _server = WiFiServer( _port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions with DHCP
+ ******************************************************************************/
+
+  //OPEN networks
+  inline int begin(char *ssid, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks
+  inline int begin(char *ssid, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _key_idx = key_idx;
+    _key = key;
+
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks
+  inline int begin(char *ssid, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _passphrase = passphrase;
+
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions without DHCP
+ ******************************************************************************/
+
+  //OPEN networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _key_idx = key_idx;
+    _key = key;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _passphrase = passphrase;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *             Stream implementations
+ ******************************************************************************/
+
+  inline int available()
+  {
+    return connect_client() ? _client.available() : 0;
+  }
+
+  inline void flush()
+  {
+    if( _client ) _client.flush();
+  }
+
+  inline int peek()
+  {
+    return connect_client() ? _client.peek(): 0;
+  }
+
+  inline int read()
+  {
+    return connect_client() ? _client.read() : -1;
+  }
+
+  inline void stop()
+  {
+    _client.stop();
+  }
+
+  inline size_t write(uint8_t byte)
+  {
+    if( connect_client() ) _client.write( byte );
+  }
+};
+
+#endif //WIFI101_STREAM_H
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.cpp
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.cpp	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.cpp	(revision 224)
@@ -0,0 +1,4 @@
+/*
+ * Implementation is in WiFiStream.h to avoid linker issues. Legacy WiFi and modern WiFi101 both define WiFiClass which
+ * will cause linker errors whenever Firmata.h is included.
+ */
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/WiFiStream.h	(revision 224)
@@ -0,0 +1,258 @@
+/*
+  WiFiStream.h
+  An Arduino Stream that wraps an instance of a WiFi server. For use
+  with legacy Arduino WiFi shield and other boards and sheilds that
+  are compatible with the Arduino WiFi library.
+
+  Copyright (C) 2015-2016 Jesse Frush. All rights reserved.
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+
+  See file LICENSE.txt for further informations on licensing terms.
+ */
+
+#ifndef WIFI_STREAM_H
+#define WIFI_STREAM_H
+
+#include <inttypes.h>
+#include <Stream.h>
+#include <WiFi.h>
+
+class WiFiStream : public Stream
+{
+private:
+  WiFiServer _server = WiFiServer(23);
+  WiFiClient _client;
+
+  //configuration members
+  IPAddress _local_ip;
+  uint16_t _port = 0;
+  uint8_t _key_idx = 0;               //WEP
+  const char *_key = nullptr;         //WEP
+  const char *_passphrase = nullptr;  //WPA
+  char *_ssid = nullptr;
+
+  inline int connect_client()
+  {
+    if( !( _client && _client.connected() ) )
+    {
+      WiFiClient newClient = _server.available();
+      if( !newClient )
+      {
+        return 0;
+      }
+
+      _client = newClient;
+    }
+    return 1;
+  }
+
+  inline bool is_ready()
+  {
+    uint8_t status = WiFi.status();
+    return !( status == WL_NO_SHIELD || status == WL_CONNECTED );
+  }
+
+public:
+  WiFiStream() {};
+
+  // allows another way to configure a static IP before begin is called
+  inline void config(IPAddress local_ip)
+  {
+    _local_ip = local_ip;
+    WiFi.config( local_ip );
+  }
+
+  // get DCHP IP
+  inline IPAddress localIP()
+  {
+    return WiFi.localIP();
+  }
+
+  inline bool maintain()
+  {
+    if( connect_client() ) return true;
+
+    stop();
+    int result = 0;
+    if( WiFi.status() != WL_CONNECTED )
+    {
+      if( _local_ip )
+      {
+        WiFi.config( _local_ip );
+      }
+
+      if( _passphrase )
+      {
+        result = WiFi.begin( _ssid, _passphrase);
+      }
+      else if( _key_idx && _key )
+      {
+        result = WiFi.begin( _ssid, _key_idx, _key );
+      }
+      else
+      {
+        result = WiFi.begin( _ssid );
+      }
+    }
+    if( result == 0 ) return false;
+
+    _server = WiFiServer( _port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions with DHCP
+ ******************************************************************************/
+
+  //OPEN networks
+  inline int begin(char *ssid, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks
+  inline int begin(char *ssid, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _key_idx = key_idx;
+    _key = key;
+
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks
+  inline int begin(char *ssid, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _passphrase = passphrase;
+
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *           Connection functions without DHCP
+ ******************************************************************************/
+
+  //OPEN networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WEP-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, uint8_t key_idx, const char *key, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _key_idx = key_idx;
+    _key = key;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, key_idx, key );
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+  //WPA-encrypted networks with static IP
+  inline int begin(char *ssid, IPAddress local_ip, const char *passphrase, uint16_t port)
+  {
+    if( !is_ready() ) return 0;
+
+    _ssid = ssid;
+    _port = port;
+    _local_ip = local_ip;
+    _passphrase = passphrase;
+
+    WiFi.config( local_ip );
+    int result = WiFi.begin( ssid, passphrase);
+    if( result == 0 ) return 0;
+
+    _server = WiFiServer( port );
+    _server.begin();
+    return result;
+  }
+
+/******************************************************************************
+ *             Stream implementations
+ ******************************************************************************/
+
+  inline int available()
+  {
+    return connect_client() ? _client.available() : 0;
+  }
+
+  inline void flush()
+  {
+    if( _client ) _client.flush();
+  }
+
+  inline int peek()
+  {
+    return connect_client() ? _client.peek(): 0;
+  }
+
+  inline int read()
+  {
+    return connect_client() ? _client.read() : -1;
+  }
+
+  inline void stop()
+  {
+    _client.stop();
+  }
+
+  inline size_t write(uint8_t byte)
+  {
+    if( connect_client() ) _client.write( byte );
+  }
+};
+
+#endif //WIFI_STREAM_H
Index: rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/firmataDebug.h
===================================================================
--- rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/firmataDebug.h	(revision 224)
+++ rtos_arduino/trunk/arduino_lib/libraries/Firmata/utility/firmataDebug.h	(revision 224)
@@ -0,0 +1,14 @@
+#ifndef FIRMATA_DEBUG_H
+#define FIRMATA_DEBUG_H
+
+#ifdef SERIAL_DEBUG
+  #define DEBUG_BEGIN(baud) Serial.begin(baud); while(!Serial) {;}
+  #define DEBUG_PRINTLN(x)  Serial.println (x)
+  #define DEBUG_PRINT(x)    Serial.print (x)
+#else
+  #define DEBUG_BEGIN(baud)
+  #define DEBUG_PRINTLN(x)
+  #define DEBUG_PRINT(x)
+#endif
+
+#endif /* FIRMATA_DEBUG_H */
