src/drivers/irlock/irlock.cpp
/** read a single block (a full frame) from sensor **/
read a single block (a full frame) from sensor
read a single block ( a full frame ) from sensor
int IRLOCK::read_device_block(struct irlock_s *block)
{
	uint8_t bytes[12];
	memset(bytes, 0, sizeof bytes);
	int status = transfer(nullptr, 0, &bytes[0], 12);
	uint16_t checksum = bytes[1] << 8 | bytes[0];
	uint16_t target_num = bytes[3] << 8 | bytes[2];
	uint16_t pixel_x = bytes[5] << 8 | bytes[4];
	uint16_t pixel_y = bytes[7] << 8 | bytes[6];
	uint16_t pixel_size_x = bytes[9] << 8 | bytes[8];
	uint16_t pixel_size_y = bytes[11] << 8 | bytes[10];
	if (target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum) {
		_read_failures++;
		return -EIO;
	}
	block->target_num = target_num;
	block->angle_x = (((float)(pixel_x - IRLOCK_CENTER_X)) / IRLOCK_PIXELS_PER_RADIAN_X);
	block->angle_y = (((float)(pixel_y - IRLOCK_CENTER_Y)) / IRLOCK_PIXELS_PER_RADIAN_Y);
	block->size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X;
	block->size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y;
	block->timestamp = hrt_absolute_time();
	return status;
}
int IRLOCK :: read_device_block ( struct irlock_s * block ) { uint8_t bytes [ 12 ] ; memset ( bytes , 0 , sizeof bytes ) ; int status = transfer ( nullptr , 0 , & bytes [ 0 ] , 12 ) ; uint16_t checksum = bytes [ 1 ] << 8 | bytes [ 0 ] ; uint16_t target_num = bytes [ 3 ] << 8 | bytes [ 2 ] ; uint16_t pixel_x = bytes [ 5 ] << 8 | bytes [ 4 ] ; uint16_t pixel_y = bytes [ 7 ] << 8 | bytes [ 6 ] ; uint16_t pixel_size_x = bytes [ 9 ] << 8 | bytes [ 8 ] ; uint16_t pixel_size_y = bytes [ 11 ] << 8 | bytes [ 10 ] ; if ( target_num + pixel_x + pixel_y + pixel_size_x + pixel_size_y != checksum ) { _read_failures ++ ; return - EIO ; } block -> target_num = target_num ; block -> angle_x = ( ( ( float ) ( pixel_x - IRLOCK_CENTER_X ) ) / IRLOCK_PIXELS_PER_RADIAN_X ) ; block -> angle_y = ( ( ( float ) ( pixel_y - IRLOCK_CENTER_Y ) ) / IRLOCK_PIXELS_PER_RADIAN_Y ) ; block -> size_x = pixel_size_x / IRLOCK_PIXELS_PER_RADIAN_X ; block -> size_y = pixel_size_y / IRLOCK_PIXELS_PER_RADIAN_Y ; block -> timestamp = hrt_absolute_time ( ) ; return status ; }
read a single block (a full frame) from sensor
read a single block ( a full frame ) from sensor
/** crc check **/ /** convert to angles **/