Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_IRLock: health reporting based on successful I2C transfer 

This change is required because sync messages are only sent if a target is visible
This commit is contained in:
Randy Mackay 2016-11-28 12:08:17 +09:00
parent 28b3de91cf
commit bac5deed43
2 changed files with 9 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AP_IRLock/AP_IRLock_I2C.cpp
View File

@ -55,6 +55,10 @@ bool AP_IRLock_I2C::sync_frame_start(void)
if (!dev->transfer(nullptr, 0, (uint8_t *)&sync_word, 4)) {
return false;
}
// record sensor successfully responded to I2C request
_last_read_ms = AP_HAL::millis();
uint8_t count=40;
while (count-- && sync_word != IRLOCK_SYNC && sync_word != 0) {
uint8_t sync_byte;
@ -90,6 +94,9 @@ bool AP_IRLock_I2C::read_block(struct frame &irframe)
return false;
}
// record sensor successfully responded to I2C request
_last_read_ms = AP_HAL::millis();
/* check crc */
uint32_t crc = irframe.signature + irframe.pixel_x + irframe.pixel_y + irframe.pixel_size_x + irframe.pixel_size_y;
if (crc != irframe.checksum) {
@ -105,8 +112,6 @@ bool AP_IRLock_I2C::read_frames(void)
return true;
}
struct frame irframe;
_last_sync_ms = AP_HAL::millis();
if (!read_block(irframe)) {
return true;
@ -157,7 +162,7 @@ bool AP_IRLock_I2C::update()
new_data = true;
}
_last_update_ms = _target_info.timestamp;
_flags.healthy = (AP_HAL::millis() - _last_sync_ms < 100);
_flags.healthy = (AP_HAL::millis() - _last_read_ms < 100);
sem->give();
}
// return true if new data found

2
libraries/AP_IRLock/AP_IRLock_I2C.h
View File

@ -36,5 +36,5 @@ private:
void pixel_to_1M_plane(float pix_x, float pix_y, float &ret_x, float &ret_y);
AP_HAL::Semaphore *sem;
uint32_t _last_sync_ms;
uint32_t _last_read_ms;
};