diff --git a/libraries/AP_HAL/board/linux.h b/libraries/AP_HAL/board/linux.h index 9df5897cc2..c9992d7349 100644 --- a/libraries/AP_HAL/board/linux.h +++ b/libraries/AP_HAL/board/linux.h @@ -26,59 +26,44 @@ #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NONE // nothing to do here #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF || CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBOARD - #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_ROLL_180_YAW_270) - #else - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_ROLL_180_YAW_90) - #endif #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BEBOP - #define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensense, 2, 0x68, ROTATION_YAW_270) #define HAL_MAG_PROBE_LIST PROBE_MAG_I2C(AK8963, 1, 0x0d, ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77, AP_Baro_MS56XX::BARO_MS5607) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_VNAV // nothing to do here #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DISCO - #define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensense, 2, 0x68, ROTATION_PITCH_180_YAW_90) #define HAL_MAG_PROBE_LIST PROBE_MAG_I2C(AK8963, 1, 0x0d, ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77, AP_Baro_MS56XX::BARO_MS5607) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77) #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIO2 - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77) #define HAL_MAG_PROBE1 PROBE_MAG_SPI(LSM9DS1, "lsm9ds1_m", ROTATION_ROLL_180) #define HAL_MAG_PROBE2 PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2 #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ERLEBRAIN2 - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_YAW_270) #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_ZYNQ // nothing to do here #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OCPOC_ZYNQ - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BBBMINI - #define HAL_INS_PROBE1 PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) - #define HAL_INS_PROBE2 PROBE_IMU_SPI(Invensense, "mpu9250ext", ROTATION_NONE) - #define HAL_INS_PROBE_LIST HAL_INS_PROBE1; HAL_INS_PROBE2 #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE1 PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_MAG_PROBE2 PROBE_MAG_IMU(AK8963, mpu9250, 1, ROTATION_NONE) #define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2 #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "icm20602", ROTATION_ROLL_180_YAW_270) #define HAL_MAG_PROBE1 PROBE_MAG_SPI(MMC5XX3, "mmc5983", false, ROTATION_YAW_180) #define HAL_MAG_PROBE2 PROBE_MAG_I2C(AK09916, 1, 0X0c, false, ROTATION_YAW_270) #define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2 @@ -86,42 +71,31 @@ #define HAL_BARO_EXTERNAL_BUS_DEFAULT 6 #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BLUE - #define HAL_INS_PROBE_LIST PROBE_IMU_I2C(Invensense, 2, 0x68, ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(BMP280, 2, 0x76) #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU_I2C(AK8963, mpu9250, 2, 0x0c, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_POCKET - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(BMP280, "bmp280") #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_BH #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77) - #define HAL_INS_PROBE1 PROBE_IMU_I2C(Invensense, 1, 0x69, ROTATION_NONE) - #define HAL_INS_PROBE2 PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) - #define HAL_INS_PROBE_LIST HAL_INS_PROBE1; HAL_INS_PROBE2 #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXFMINI - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_YAW_270) #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_AERO - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(BMI160, "bmi160") #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 2, 0x76) #define HAL_MAG_PROBE1 PROBE_MAG_I2C(BMM150, 2, 0x12, ROTATION_NONE) #define HAL_MAG_PROBE2 PROBE_MAG_I2C(HMC5843, 4, 0x1e, true, ROTATION_NONE) #define HAL_MAG_PROBE3 PROBE_MAG_I2C(IST8310, 4, 0x0e, true, ROTATION_PITCH_180_YAW_90) #define HAL_MAG_PROBE_LIST HAL_MAG_PROBE1; HAL_MAG_PROBE2; HAL_MAG_PROBE3 #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_DARK - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(MS56XX, 1, 0x77) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_EDGE - #define HAL_INS_PROBE1 PROBE_IMU_SPI(Invensense, "mpu60x0", ROTATION_YAW_90) - #define HAL_INS_PROBE2 PROBE_IMU_SPI(Invensense, "mpu60x0ext", ROTATION_YAW_90) - #define HAL_INS_PROBE_LIST HAL_INS_PROBE1; HAL_INS_PROBE2 #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") // only external compasses #define HAL_PROBE_EXTERNAL_I2C_COMPASSES @@ -130,11 +104,9 @@ #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_SPI(LIS3MDL, lis3mdl, false, ROTATION_ROLL_180_YAW_90) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_CANZERO - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_SPI(MS56XX, "ms5611") #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #elif CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_OBAL_V1 - #define HAL_INS_PROBE_LIST PROBE_IMU_SPI(Invensense, "mpu9250", ROTATION_NONE) #define HAL_MAG_PROBE_LIST PROBE_MAG_IMU(AK8963, mpu9250, 0, ROTATION_NONE) #define HAL_BARO_PROBE_LIST PROBE_BARO_I2C(BMP085, 1, 0x77) #define HAL_PROBE_EXTERNAL_I2C_COMPASSES diff --git a/libraries/AP_HAL/hwdef/scripts/hwdef.py b/libraries/AP_HAL/hwdef/scripts/hwdef.py index 75434ce6e7..2326960e74 100644 --- a/libraries/AP_HAL/hwdef/scripts/hwdef.py +++ b/libraries/AP_HAL/hwdef/scripts/hwdef.py @@ -41,6 +41,9 @@ class HWDef: # boolean indicating whether we have read and processed self.hwdef self.processed_hwdefs = False + # sensor lists + self.imu_list = [] + def is_int(self, str): '''check if a string is an integer''' try: @@ -194,6 +197,9 @@ class HWDef: if a[0] == 'define': return self.process_line_define(line, depth, a) + elif a[0] == 'IMU': + self.imu_list.append(a[1:]) + def process_line_undef(self, line, depth, a): for u in a[1:]: self.progress("Removing %s" % u) @@ -203,6 +209,8 @@ class HWDef: for line in self.alllines[:]: if line.startswith('define') and u == line.split()[1]: self.alllines.remove(line) + if u == 'IMU': + self.imu_list = [] def process_line_env(self, line, depth, a): self.progress("Adding environment %s" % ' '.join(a[1:])) @@ -224,3 +232,77 @@ class HWDef: # raise ValueError(msg) self.intdefines[name] = intvalue + + def parse_spi_device(self, dev): + '''parse a SPI:xxx device item''' + a = dev.split(':') + if len(a) != 2: + self.error("Bad SPI device: %s" % dev) + return 'hal.spi->get_device("%s")' % a[1] + + def parse_i2c_device(self, dev): + '''parse a I2C:xxx:xxx device item''' + a = dev.split(':') + if len(a) != 3: + self.error("Bad I2C device: %s" % dev) + busaddr = int(a[2], base=0) + if a[1] == 'ALL_EXTERNAL': + return ('FOREACH_I2C_EXTERNAL(b)', 'GET_I2C_DEVICE(b,0x%02x)' % (busaddr)) + elif a[1] == 'ALL_INTERNAL': + return ('FOREACH_I2C_INTERNAL(b)', 'GET_I2C_DEVICE(b,0x%02x)' % (busaddr)) + elif a[1] == 'ALL': + return ('FOREACH_I2C(b)', 'GET_I2C_DEVICE(b,0x%02x)' % (busaddr)) + busnum = int(a[1]) + return ('', 'GET_I2C_DEVICE(%u,0x%02x)' % (busnum, busaddr)) + + def seen_str(self, dev): + '''return string representation of device for checking for duplicates''' + ret = dev[:2] + if dev[-1].startswith("BOARD_MATCH("): + ret.append(dev[-1]) + return str(ret) + + def write_IMU_config(self, f): + '''write IMU config defines''' + devlist = [] + wrapper = '' + seen = set() + for dev in self.imu_list: + if self.seen_str(dev) in seen: + self.error("Duplicate IMU: %s" % self.seen_str(dev)) + seen.add(self.seen_str(dev)) + driver = dev[0] + # get instance number if mentioned + instance = -1 + aux_devid = -1 + if dev[-1].startswith("INSTANCE:"): + instance = int(dev[-1][9:]) + dev = dev[:-1] + if dev[-1].startswith("AUX:"): + aux_devid = int(dev[-1][4:]) + dev = dev[:-1] + for i in range(1, len(dev)): + if dev[i].startswith("SPI:"): + dev[i] = self.parse_spi_device(dev[i]) + elif dev[i].startswith("I2C:"): + (wrapper, dev[i]) = self.parse_i2c_device(dev[i]) + n = len(devlist)+1 + devlist.append('HAL_INS_PROBE%u' % n) + if aux_devid != -1: + f.write('#define HAL_INS_PROBE%u %s ADD_BACKEND_AUX(AP_InertialSensor_%s::probe(*this,%s),%d)\n' % + (n, wrapper, driver, ','.join(dev[1:]), aux_devid)) + elif instance != -1: + f.write('#define HAL_INS_PROBE%u %s ADD_BACKEND_INSTANCE(AP_InertialSensor_%s::probe(*this,%s),%d)\n' % + (n, wrapper, driver, ','.join(dev[1:]), instance)) + elif dev[-1].startswith("BOARD_MATCH("): + f.write( + '#define HAL_INS_PROBE%u %s ADD_BACKEND_BOARD_MATCH(%s, AP_InertialSensor_%s::probe(*this,%s))\n' + % (n, wrapper, dev[-1], driver, ','.join(dev[1:-1]))) + else: + f.write( + '#define HAL_INS_PROBE%u %s ADD_BACKEND(AP_InertialSensor_%s::probe(*this,%s))\n' + % (n, wrapper, driver, ','.join(dev[1:]))) + if len(devlist) > 0: + if len(devlist) < 3: + self.write_defaulting_define(f, 'INS_MAX_INSTANCES', len(devlist)) + f.write('#define HAL_INS_PROBE_LIST %s\n\n' % ';'.join(devlist))