Firmata for Raspberry Pi Pico

I noticed that there was not support for the Raspberry Pi Pico and I stated implementing it. I have tested Digital I/O, Analog Out and Servo so far. However I hit a roadblock with Analog Inputs. This is what I have so far:

#elif defined(ARDUINO_RASPBERRY_PI_PICO)
#define TOTAL_ANALOG_PINS       3
#define TOTAL_PINS              29 
#define VERSION_BLINK_PIN       25
#define PIN_SERIAL1_RX          0
#define PIN_SERIAL1_TX          1
#define IS_PIN_DIGITAL(p)       (((p) >= 0 && (p) <= 22) || ((p) >= 25 && (p  <= 28)))
#define IS_PIN_ANALOG(p)        (((p) >= 26 && (p) <= 28))
#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 IS_PIN_SERIAL(p)        (((p) > 0 && (p) < 1))
#define PIN_TO_DIGITAL(p)       (p)
#define PIN_TO_ANALOG(p)        ((p) - 26)
#define PIN_TO_PWM(p)           PIN_TO_DIGITAL(p)
#define PIN_TO_SERVO(p)         (p)

The problem seems to be that in the Raspberry Pi Pico reading the ADC0 needs to be done by reading the pin 26 analogRead(26). But in the StandarFirmata.ino the macro PIN_TO_ANALOG is used to get the corresponding analog pin counting from zero (A0). Here is the corresponding code:

/* 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));
        }
      }
    }

With the macro as I have defined it (PIN_TO_ANALOG(p) ((p) - 26)), for pin 26 it calls analogRead(0) which is incorrect. If I change the code to be as follows:

Firmata.sendAnalog(analogPin, analogRead(26 + analogPin));

if works fine and I'm able to read the analog pins.

I tried changing the macros to be as follows:

#define TOTAL_ANALOG_PINS       29
#define PIN_TO_ANALOG(p)        (p)

I was hoping to get the code to call analogRead(26) but it does not work. I guess that it is because TOTAL_ANALOG_PINS need to be maximum 16.

Is there a way of fixing this without needing to modify the StandarFirmata code?

firmata / arduino

Firmata for Raspberry Pi Pico #513