AP_Airspeed: Don't fail on REG_WHOAMI_RECHECK_ID

This prevents a false negative ID check for the cases when the AP is rebooted independently of the sensor, with the latter keeping a temporary ID that was set during initialization
This commit is contained in:
Oleksiy Protas 2024-09-13 15:40:41 +03:00 committed by Andrew Tridgell
parent 0cc07ac1ab
commit 94f2eb4999
1 changed files with 1 additions and 1 deletions

View File

@ -78,7 +78,7 @@ bool AP_Airspeed_ASP5033::confirm_sensor_id(void)
{ {
uint8_t part_id; uint8_t part_id;
if (!dev->read_registers(REG_PART_ID_SET, &part_id, 1) || if (!dev->read_registers(REG_PART_ID_SET, &part_id, 1) ||
part_id != REG_WHOAMI_DEFAULT_ID) { ( (part_id != REG_WHOAMI_DEFAULT_ID) && (part_id != REG_WHOAMI_RECHECK_ID) ) ) {
return false; return false;
} }
if (!dev->write_register(REG_PART_ID_SET, REG_WHOAMI_RECHECK_ID)) { if (!dev->write_register(REG_PART_ID_SET, REG_WHOAMI_RECHECK_ID)) {