HAL_Linux: include readRegistersMultiple in I2CDriver

AP_HAL_Linux implemments this new method.

libraries/AP_HAL/I2CDriver.h

@@ -26,11 +26,18 @@ public:
     /* readRegister: read from a device register - writes the register,
      * then reads back an 8-bit value. */
     virtual uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data) = 0;
-    /* readRegister: read contigious device registers - writes the first 
+
+    /* readRegisters: read contigious device registers - writes the first 
      * register, then reads back multiple bytes */
     virtual uint8_t readRegisters(uint8_t addr, uint8_t reg,
                                   uint8_t len, uint8_t* data) = 0;
 
+    /* readRegistersMultiple: read contigious device registers. 
+       Equivalent to count calls to readRegisters() */
+    virtual uint8_t readRegistersMultiple(uint8_t addr, uint8_t reg,
+                                          uint8_t len, uint8_t count, 
+                                          uint8_t* data) = 0;
+
     virtual uint8_t lockup_count() = 0;
     void ignore_errors(bool b) { _ignore_errors = b; }
     virtual AP_HAL::Semaphore* get_semaphore() = 0;

libraries/AP_HAL_Linux/I2CDriver.cpp

@@ -137,15 +137,59 @@ uint8_t LinuxI2CDriver::read(uint8_t addr, uint8_t len, uint8_t* data)
 uint8_t LinuxI2CDriver::readRegisters(uint8_t addr, uint8_t reg,
                                       uint8_t len, uint8_t* data)
 {
-    if (!set_address(addr)) {
+    struct i2c_msg msgs[] = {
+        {
+        addr  : addr,
+        flags : 0,
+        len   : 1,
+        buf   : &reg
+        },
+        {
+        addr  : addr,
+        flags : I2C_M_RD,
+        len   : len,
+        buf   : data,
+        }
+    };
+    struct i2c_rdwr_ioctl_data i2c_data = {
+    msgs : msgs,
+    nmsgs : 2
+    };
+
+    if (ioctl(_fd, I2C_RDWR, &i2c_data) == -1) {
         return 1;
     }
-    // send the address to read from
+
-    if (::write(_fd, &reg, 1) != 1) {
+    return 0;
-        return 1;
+}
-    }
+
-    if (::read(_fd, data, len) != len) {
+
-        return 1;
+uint8_t LinuxI2CDriver::readRegistersMultiple(uint8_t addr, uint8_t reg,
+                                              uint8_t len, 
+                                              uint8_t count, uint8_t* data)
+{
+    while (count > 0) {
+        uint8_t n = count>8?8:count;
+        struct i2c_msg msgs[2*n];
+        struct i2c_rdwr_ioctl_data i2c_data = {
+        msgs : msgs,
+        nmsgs : 2*n
+        };
+        for (uint8_t i=0; i<n; i++) {
+            msgs[i*2].addr = addr;
+            msgs[i*2].flags = 0;
+            msgs[i*2].len = 1;
+            msgs[i*2].buf = &reg;
+            msgs[i*2+1].addr = addr;
+            msgs[i*2+1].flags = I2C_M_RD;
+            msgs[i*2+1].len = len;
+            msgs[i*2+1].buf = data;
+            data += len;
+        };
+        if (ioctl(_fd, I2C_RDWR, &i2c_data) == -1) {
+            return 1;
+        }
+        count -= n;
     }
     return 0;
 }

libraries/AP_HAL_Linux/I2CDriver.h

@@ -26,11 +26,16 @@ public:
     /* readRegister: read from a device register - writes the register,
      * then reads back an 8-bit value. */
     uint8_t readRegister(uint8_t addr, uint8_t reg, uint8_t* data);
+
     /* readRegister: read contigious device registers - writes the first 
      * register, then reads back multiple bytes */
     uint8_t readRegisters(uint8_t addr, uint8_t reg,
                                   uint8_t len, uint8_t* data);
 
+    uint8_t readRegistersMultiple(uint8_t addr, uint8_t reg,
+                                  uint8_t len, uint8_t count, 
+                                  uint8_t* data);
+
     uint8_t lockup_count();
 
     AP_HAL::Semaphore* get_semaphore() { return _semaphore; }

libraries/AP_HAL_Linux/Scheduler.cpp

@@ -227,7 +227,7 @@ void *LinuxScheduler::_timer_thread(void)
         poll(NULL, 0, 1);        
     }
     while (true) {
-        _microsleep(5000);
+        _microsleep(1000);
 
         // run registered timers
         _run_timers(true);