AP_HAL_Linux: use I2C_SLAVE_FORCE in case of error

When there is already a driver registered on an i2c bus, the I2C_SLAVE ioctl
returns an error.
When it happens, it is better to display a warning and try to force the address.
It is especially useful on the bebop when killing the regular autopilot that uses
iio drivers to access the imu because else we would need to manually unbind the
driver in an init procedure.

I have added a warning because this error can also be resulting of another cause.
If the error is not EBUSY, then panic

If the I2C_SLAVE_FORCE ioctl fails then we panic because one of the i2c devices
won't be working properly.

libraries/AP_HAL_Linux/I2CDriver.cpp

@@ -120,10 +120,27 @@ bool I2CDriver::set_address(uint8_t addr)
     if (_fd == -1) {
         return false;
     }
-    if (_addr != addr) {
-        ioctl(_fd, I2C_SLAVE, addr);
+    if (_addr == addr) {
+        goto end;
+    } else {
+        if (ioctl(_fd, I2C_SLAVE, addr) < 0) {
+            if (errno != EBUSY) {
+                return false;
+            }
+            /* Only print this message once per i2c bus */
+            if (_print_ioctl_error) {
+                hal.console->printf("couldn't set i2c slave address: %s\n",
+                                    strerror(errno));
+                hal.console->printf("trying I2C_SLAVE_FORCE\n");
+                _print_ioctl_error = false;
+            }
+            if (ioctl(_fd, I2C_SLAVE_FORCE, addr) < 0) {
+                return false;
+            }
+        }
         _addr = addr;
     }
+end:
     return true;
 }
 

libraries/AP_HAL_Linux/I2CDriver.h

@@ -49,6 +49,7 @@ private:
     char *_device = NULL;
     int _fd = -1;
     uint8_t _addr;
+    bool _print_ioctl_error = true;
 };
 
 #endif // __AP_HAL_LINUX_I2CDRIVER_H__