diff --git a/libraries/AP_Airspeed/AP_Airspeed.cpp b/libraries/AP_Airspeed/AP_Airspeed.cpp index 13c47b361d..481d029770 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed.cpp @@ -53,7 +53,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = { // @Param: TYPE // @DisplayName: Airspeed type // @Description: Type of airspeed sensor - // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525 + // @Values: 0:None,1:I2C-MS4525D0,2:Analog,3:I2C-MS5525,4:I2C-MS5525 (0x76),5:I2C-MS5525 (0x77) // @User: Standard AP_GROUPINFO_FLAGS("TYPE", 0, AP_Airspeed, _type, ARSPD_DEFAULT_TYPE, AP_PARAM_FLAG_ENABLE), @@ -158,7 +158,13 @@ void AP_Airspeed::init() sensor = new AP_Airspeed_Analog(*this); break; case TYPE_I2C_MS5525: - sensor = new AP_Airspeed_MS5525(*this); + sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_AUTO); + break; + case TYPE_I2C_MS5525_ADDRESS_1: + sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_1); + break; + case TYPE_I2C_MS5525_ADDRESS_2: + sensor = new AP_Airspeed_MS5525(*this, AP_Airspeed_MS5525::MS5525_ADDR_2); break; } if (sensor && !sensor->init()) { diff --git a/libraries/AP_Airspeed/AP_Airspeed.h b/libraries/AP_Airspeed/AP_Airspeed.h index 62e03503d3..0e87c0d5fa 100644 --- a/libraries/AP_Airspeed/AP_Airspeed.h +++ b/libraries/AP_Airspeed/AP_Airspeed.h @@ -145,6 +145,8 @@ public: TYPE_I2C_MS4525=1, TYPE_ANALOG=2, TYPE_I2C_MS5525=3, + TYPE_I2C_MS5525_ADDRESS_1=4, + TYPE_I2C_MS5525_ADDRESS_2=5, }; private: diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp index b93562fef5..9ccc533154 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.cpp @@ -51,9 +51,10 @@ extern const AP_HAL::HAL &hal; #define REG_CONVERT_PRESSURE REG_CONVERT_D1_OSR_1024 #define REG_CONVERT_TEMPERATURE REG_CONVERT_D2_OSR_1024 -AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend) : +AP_Airspeed_MS5525::AP_Airspeed_MS5525(AP_Airspeed &_frontend, MS5525_ADDR address) : AP_Airspeed_Backend(_frontend) { + _address = address; } // probe and initialise the sensor @@ -62,6 +63,9 @@ bool AP_Airspeed_MS5525::init() const uint8_t addresses[] = { MS5525D0_I2C_ADDR_1, MS5525D0_I2C_ADDR_2 }; bool found = false; for (uint8_t i=0; iget_device(get_bus(), addresses[i]); if (!dev) { continue; diff --git a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h index 0cf9bbc708..1370c906ca 100644 --- a/libraries/AP_Airspeed/AP_Airspeed_MS5525.h +++ b/libraries/AP_Airspeed/AP_Airspeed_MS5525.h @@ -30,7 +30,13 @@ class AP_Airspeed_MS5525 : public AP_Airspeed_Backend { public: - AP_Airspeed_MS5525(AP_Airspeed &frontend); + enum MS5525_ADDR { + MS5525_ADDR_1 = 0, + MS5525_ADDR_2 = 1, + MS5525_ADDR_AUTO = 255, // does not need to be 255, just needs to be out of the address array + }; + + AP_Airspeed_MS5525(AP_Airspeed &frontend, MS5525_ADDR address); ~AP_Airspeed_MS5525(void) {} // probe and initialise the sensor @@ -67,6 +73,7 @@ private: uint32_t command_send_us; bool ignore_next; uint8_t cmd_sent; + MS5525_ADDR _address; AP_HAL::OwnPtr dev; };