Skip to content

Commit e1365de

Browse files
zarvoxzarvox
committed
cameras.c: memcpy structs into properly aligned buffers before toggling endianness.
The new registration structs are not aligned to 4-byte boundaries in the buffers that we get back from the Kinect. This might have been causing breakage on ARM, where nonaligned access is prohibited. Signed-off-by: Drew Fisher <[email protected]>
1 parent 3629b75 commit e1365de

File tree

1 file changed

+67
-65
lines changed

1 file changed

+67
-65
lines changed

‎src/cameras.c‎

Lines changed: 67 additions & 65 deletions
Original file line numberDiff line numberDiff line change
@@ -783,56 +783,55 @@ static int freenect_fetch_reg_info(freenect_device *dev)
783783
FN_ERROR("freenect_fetch_reg_info: send_cmd read %d bytes (expected 118)\n", res);
784784
return -1;
785785
}
786-
freenect_reg_info *reg_info_ptr = (freenect_reg_info*)(&reply[2]);
787-
freenect_reg_info *dev_reg_info = &(dev->registration.reg_info);
788-
dev_reg_info->ax = fn_le32s(reg_info_ptr->ax);
789-
dev_reg_info->bx = fn_le32s(reg_info_ptr->bx);
790-
dev_reg_info->cx = fn_le32s(reg_info_ptr->cx);
791-
dev_reg_info->dx = fn_le32s(reg_info_ptr->dx);
792-
dev_reg_info->ay = fn_le32s(reg_info_ptr->ay);
793-
dev_reg_info->by = fn_le32s(reg_info_ptr->by);
794-
dev_reg_info->cy = fn_le32s(reg_info_ptr->cy);
795-
dev_reg_info->dy = fn_le32s(reg_info_ptr->dy);
796-
dev_reg_info->dx_start = fn_le32s(reg_info_ptr->dx_start);
797-
dev_reg_info->dy_start = fn_le32s(reg_info_ptr->dy_start);
798-
dev_reg_info->dx_beta_start = fn_le32s(reg_info_ptr->dx_beta_start);
799-
dev_reg_info->dy_beta_start = fn_le32s(reg_info_ptr->dy_beta_start);
800-
dev_reg_info->dx_beta_inc = fn_le32s(reg_info_ptr->dx_beta_inc);
801-
dev_reg_info->dy_beta_inc = fn_le32s(reg_info_ptr->dy_beta_inc);
802-
dev_reg_info->dxdx_start = fn_le32s(reg_info_ptr->dxdx_start);
803-
dev_reg_info->dxdy_start = fn_le32s(reg_info_ptr->dxdy_start);
804-
dev_reg_info->dydx_start = fn_le32s(reg_info_ptr->dydx_start);
805-
dev_reg_info->dydy_start = fn_le32s(reg_info_ptr->dydy_start);
806-
dev_reg_info->dxdxdx_start = fn_le32s(reg_info_ptr->dxdxdx_start);
807-
dev_reg_info->dydxdx_start = fn_le32s(reg_info_ptr->dydxdx_start);
808-
dev_reg_info->dxdxdy_start = fn_le32s(reg_info_ptr->dxdxdy_start);
809-
dev_reg_info->dydxdy_start = fn_le32s(reg_info_ptr->dydxdy_start);
810-
dev_reg_info->dydydx_start = fn_le32s(reg_info_ptr->dydydx_start);
811-
dev_reg_info->dydydy_start = fn_le32s(reg_info_ptr->dydydy_start);
812-
FN_SPEW("ax: %d\n", dev_reg_info->ax);
813-
FN_SPEW("bx: %d\n", dev_reg_info->bx);
814-
FN_SPEW("cx: %d\n", dev_reg_info->cx);
815-
FN_SPEW("dx: %d\n", dev_reg_info->dx);
816-
FN_SPEW("ay: %d\n", dev_reg_info->ay);
817-
FN_SPEW("by: %d\n", dev_reg_info->by);
818-
FN_SPEW("cy: %d\n", dev_reg_info->cy);
819-
FN_SPEW("dy: %d\n", dev_reg_info->dy);
820-
FN_SPEW("dx_start: %d\n", dev_reg_info->dx_start);
821-
FN_SPEW("dy_start: %d\n", dev_reg_info->dy_start);
822-
FN_SPEW("dx_beta_start: %d\n", dev_reg_info->dx_beta_start);
823-
FN_SPEW("dy_beta_start: %d\n", dev_reg_info->dy_beta_start);
824-
FN_SPEW("dx_beta_inc: %d\n", dev_reg_info->dx_beta_inc);
825-
FN_SPEW("dy_beta_inc: %d\n", dev_reg_info->dy_beta_inc);
826-
FN_SPEW("dxdx_start: %d\n", dev_reg_info->dxdx_start);
827-
FN_SPEW("dxdy_start: %d\n", dev_reg_info->dxdy_start);
828-
FN_SPEW("dydx_start: %d\n", dev_reg_info->dydx_start);
829-
FN_SPEW("dydy_start: %d\n", dev_reg_info->dydy_start);
830-
FN_SPEW("dxdxdx_start: %d\n", dev_reg_info->dxdxdx_start);
831-
FN_SPEW("dydxdx_start: %d\n", dev_reg_info->dydxdx_start);
832-
FN_SPEW("dxdxdy_start: %d\n", dev_reg_info->dxdxdy_start);
833-
FN_SPEW("dydxdy_start: %d\n", dev_reg_info->dydxdy_start);
834-
FN_SPEW("dydydx_start: %d\n", dev_reg_info->dydydx_start);
835-
FN_SPEW("dydydy_start: %d\n", dev_reg_info->dydydy_start);
786+
memcpy(&dev->registration.reg_info, reply + 2, sizeof(dev->registration.reg_info));
787+
dev->registration.reg_info.ax = fn_le32s(dev->registration.reg_info.ax);
788+
dev->registration.reg_info.bx = fn_le32s(dev->registration.reg_info.bx);
789+
dev->registration.reg_info.cx = fn_le32s(dev->registration.reg_info.cx);
790+
dev->registration.reg_info.dx = fn_le32s(dev->registration.reg_info.dx);
791+
dev->registration.reg_info.ay = fn_le32s(dev->registration.reg_info.ay);
792+
dev->registration.reg_info.by = fn_le32s(dev->registration.reg_info.by);
793+
dev->registration.reg_info.cy = fn_le32s(dev->registration.reg_info.cy);
794+
dev->registration.reg_info.dy = fn_le32s(dev->registration.reg_info.dy);
795+
dev->registration.reg_info.dx_start = fn_le32s(dev->registration.reg_info.dx_start);
796+
dev->registration.reg_info.dy_start = fn_le32s(dev->registration.reg_info.dy_start);
797+
dev->registration.reg_info.dx_beta_start = fn_le32s(dev->registration.reg_info.dx_beta_start);
798+
dev->registration.reg_info.dy_beta_start = fn_le32s(dev->registration.reg_info.dy_beta_start);
799+
dev->registration.reg_info.dx_beta_inc = fn_le32s(dev->registration.reg_info.dx_beta_inc);
800+
dev->registration.reg_info.dy_beta_inc = fn_le32s(dev->registration.reg_info.dy_beta_inc);
801+
dev->registration.reg_info.dxdx_start = fn_le32s(dev->registration.reg_info.dxdx_start);
802+
dev->registration.reg_info.dxdy_start = fn_le32s(dev->registration.reg_info.dxdy_start);
803+
dev->registration.reg_info.dydx_start = fn_le32s(dev->registration.reg_info.dydx_start);
804+
dev->registration.reg_info.dydy_start = fn_le32s(dev->registration.reg_info.dydy_start);
805+
dev->registration.reg_info.dxdxdx_start = fn_le32s(dev->registration.reg_info.dxdxdx_start);
806+
dev->registration.reg_info.dydxdx_start = fn_le32s(dev->registration.reg_info.dydxdx_start);
807+
dev->registration.reg_info.dxdxdy_start = fn_le32s(dev->registration.reg_info.dxdxdy_start);
808+
dev->registration.reg_info.dydxdy_start = fn_le32s(dev->registration.reg_info.dydxdy_start);
809+
dev->registration.reg_info.dydydx_start = fn_le32s(dev->registration.reg_info.dydydx_start);
810+
dev->registration.reg_info.dydydy_start = fn_le32s(dev->registration.reg_info.dydydy_start);
811+
FN_SPEW("ax: %d\n", dev->registration.reg_info.ax);
812+
FN_SPEW("bx: %d\n", dev->registration.reg_info.bx);
813+
FN_SPEW("cx: %d\n", dev->registration.reg_info.cx);
814+
FN_SPEW("dx: %d\n", dev->registration.reg_info.dx);
815+
FN_SPEW("ay: %d\n", dev->registration.reg_info.ay);
816+
FN_SPEW("by: %d\n", dev->registration.reg_info.by);
817+
FN_SPEW("cy: %d\n", dev->registration.reg_info.cy);
818+
FN_SPEW("dy: %d\n", dev->registration.reg_info.dy);
819+
FN_SPEW("dx_start: %d\n", dev->registration.reg_info.dx_start);
820+
FN_SPEW("dy_start: %d\n", dev->registration.reg_info.dy_start);
821+
FN_SPEW("dx_beta_start: %d\n", dev->registration.reg_info.dx_beta_start);
822+
FN_SPEW("dy_beta_start: %d\n", dev->registration.reg_info.dy_beta_start);
823+
FN_SPEW("dx_beta_inc: %d\n", dev->registration.reg_info.dx_beta_inc);
824+
FN_SPEW("dy_beta_inc: %d\n", dev->registration.reg_info.dy_beta_inc);
825+
FN_SPEW("dxdx_start: %d\n", dev->registration.reg_info.dxdx_start);
826+
FN_SPEW("dxdy_start: %d\n", dev->registration.reg_info.dxdy_start);
827+
FN_SPEW("dydx_start: %d\n", dev->registration.reg_info.dydx_start);
828+
FN_SPEW("dydy_start: %d\n", dev->registration.reg_info.dydy_start);
829+
FN_SPEW("dxdxdx_start: %d\n", dev->registration.reg_info.dxdxdx_start);
830+
FN_SPEW("dydxdx_start: %d\n", dev->registration.reg_info.dydxdx_start);
831+
FN_SPEW("dxdxdy_start: %d\n", dev->registration.reg_info.dxdxdy_start);
832+
FN_SPEW("dydxdy_start: %d\n", dev->registration.reg_info.dydxdy_start);
833+
FN_SPEW("dydydx_start: %d\n", dev->registration.reg_info.dydydx_start);
834+
FN_SPEW("dydydy_start: %d\n", dev->registration.reg_info.dydydy_start);
836835
/*
837836
// NOTE: Not assigned above
838837
FN_SPEW("dx_center: %d\n", dev_reg_info->dx_center);
@@ -861,10 +860,10 @@ static int freenect_fetch_reg_pad_info(freenect_device *dev)
861860
FN_ERROR("freenect_fetch_reg_pad_info: send_cmd read %d bytes (expected 8)\n", res);
862861
return -1;
863862
}
864-
freenect_reg_pad_info *pad_info_ptr = (freenect_reg_pad_info*)(&reply[2]);
865-
dev->registration.reg_pad_info.start_lines = fn_le16s(pad_info_ptr->start_lines);
866-
dev->registration.reg_pad_info.end_lines = fn_le16s(pad_info_ptr->end_lines);
867-
dev->registration.reg_pad_info.cropping_lines = fn_le16s(pad_info_ptr->cropping_lines);
863+
memcpy(&dev->registration.reg_pad_info, reply+2, sizeof(dev->registration.reg_pad_info));
864+
dev->registration.reg_pad_info.start_lines = fn_le16s(dev->registration.reg_pad_info.start_lines);
865+
dev->registration.reg_pad_info.end_lines = fn_le16s(dev->registration.reg_pad_info.end_lines);
866+
dev->registration.reg_pad_info.cropping_lines = fn_le16s(dev->registration.reg_pad_info.cropping_lines);
868867
FN_SPEW("start_lines: %u\n",dev->registration.reg_pad_info.start_lines);
869868
FN_SPEW("end_lines: %u\n",dev->registration.reg_pad_info.end_lines);
870869
FN_SPEW("cropping_lines: %u\n",dev->registration.reg_pad_info.cropping_lines);
@@ -888,7 +887,9 @@ static int freenect_fetch_reg_const_shift(freenect_device *dev)
888887
FN_ERROR("freenect_fetch_reg_const_shift: send_cmd read %d bytes (expected 8)\n", res);
889888
return -1;
890889
}
891-
uint16_t shift = fn_le16(*((uint16_t*)(reply+2)));
890+
uint16_t shift;
891+
memcpy(&shift, reply+2, sizeof(shift));
892+
shift = fn_le16(shift);
892893
dev->registration.const_shift = (double)shift;
893894
FN_SPEW("const_shift: %f\n",dev->registration.const_shift);
894895
return 0;
@@ -907,17 +908,18 @@ static int freenect_fetch_zero_plane_info(freenect_device *dev)
907908
FN_ERROR("freenect_fetch_zero_plane_info: send_cmd read %d bytes (expected 322)\n", res);
908909
return -1;
909910
}
911+
912+
memcpy(&(dev->registration.zero_plane_info), reply + 94, sizeof(dev->registration.zero_plane_info));
913+
dev->registration.zero_plane_info.dcmos_emitter_dist = *((float*)(&fn_le32(*((uint32_t*)(&dev->registration.zero_plane_info.dcmos_emitter_dist)))));
914+
dev->registration.zero_plane_info.dcmos_rcmos_dist = *((float*)(&fn_le32(*((uint32_t*)(&dev->registration.zero_plane_info.dcmos_rcmos_dist)))));
915+
dev->registration.zero_plane_info.reference_distance = *((float*)(&fn_le32(*((uint32_t*)(&dev->registration.zero_plane_info.reference_distance)))));
916+
dev->registration.zero_plane_info.reference_pixel_size = *((float*)(&fn_le32(*((uint32_t*)(&dev->registration.zero_plane_info.reference_pixel_size)))));
917+
910918
// WTF is all this data? it's way bigger than sizeof(XnFixedParams)...
911-
FN_SPEW("dcmos_emitter_distance: %f\n", *((float*)(reply+94)));
912-
FN_SPEW("dcmos_rcmos_distance: %f\n", *((float*)(reply+98)));
913-
FN_SPEW("reference_distance: %f\n", *((float*)(reply+102)));
914-
FN_SPEW("reference_pixel_size: %f\n", *((float*)(reply+106)));
915-
916-
// The values are 32-bit floats in little-endian. So:
917-
dev->registration.zero_plane_info.dcmos_emitter_dist = *((float*)(&fn_le32(*((uint32_t*)(reply+94)))));
918-
dev->registration.zero_plane_info.dcmos_rcmos_dist = *((float*)(&fn_le32(*((uint32_t*)(reply+98)))));
919-
dev->registration.zero_plane_info.reference_distance = *((float*)(&fn_le32(*((uint32_t*)(reply+102)))));
920-
dev->registration.zero_plane_info.reference_pixel_size = *((float*)(&fn_le32(*((uint32_t*)(reply+106)))));
919+
FN_SPEW("dcmos_emitter_distance: %f\n", dev->registration.zero_plane_info.dcmos_emitter_dist);
920+
FN_SPEW("dcmos_rcmos_distance: %f\n", dev->registration.zero_plane_info.dcmos_rcmos_dist);
921+
FN_SPEW("reference_distance: %f\n", dev->registration.zero_plane_info.reference_distance);
922+
FN_SPEW("reference_pixel_size: %f\n", dev->registration.zero_plane_info.reference_pixel_size);
921923

922924
// FIXME: OpenNI seems to use a hardcoded value of 2.4 instead of 2.3 as reported by Kinect
923925
dev->registration.zero_plane_info.dcmos_rcmos_dist = 2.4;

0 commit comments

Comments
 (0)