I2C_STOP_READING defaults to stopping first query index if address not found

[TeamWildCards]

https://github.com/firmata/arduino/blob/master/examples/StandardFirmata/StandardFirmata.ino

Starting on line 560:

        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--;

The default assignment queryIndexToSkip = 0 means that if the specified slave address is not found, then query[0] is going to be removed from the list of I2C queries.

This can be fixed by making queryIndexToSkip a signed char instead of a byte (which is a better match to queryIndex anyway), defaulting queryIndexToSkip = -1, and then checking to see if queryIndexToSkip >= 0 before executing the second for loop and the queryIndex decrement.

I can generate a pull request for this, but I wanted to make sure first that (1) it is indeed considered a bug, (2) it is worthwhile to fix, as no one seems to have reported this issue thus far, and (3) it is OK that I haven't actually tested this--I came across it while reading through the code, and not actually testing it on hardware.

[soundanalogous]

This is definitely a bug but I'm not sure if it's worth fixing in isolation. A user would only encounter this bug if they attempted to send an I2C_STOP_READING command without first sending an I2C_READ_CONTINUOUSLY command since the matching slaveAddress is specified for that command.

A proper fix here would require much more defensive coding throughout StandardFirmata and revamping the error messaging scheme (since the existing String-based errors use a fair amount of memory).