void alignSensors(int32_t *src, int32_t *dest, uint8_t rotation) { static uint32_t swap[3]; memcpy(swap, src, sizeof(swap)); switch (rotation) { default: case CW0_DEG: dest[X] = swap[X]; dest[Y] = swap[Y]; dest[Z] = swap[Z]; break; case CW90_DEG: dest[X] = swap[Y]; dest[Y] = -swap[X]; dest[Z] = swap[Z]; break; case CW180_DEG: dest[X] = -swap[X]; dest[Y] = -swap[Y]; dest[Z] = swap[Z]; break; case CW270_DEG: dest[X] = -swap[Y]; dest[Y] = swap[X]; dest[Z] = swap[Z]; break; case CW0_DEG_FLIP: dest[X] = -swap[X]; dest[Y] = swap[Y]; dest[Z] = -swap[Z]; break; case CW90_DEG_FLIP: dest[X] = swap[Y]; dest[Y] = swap[X]; dest[Z] = -swap[Z]; break; case CW180_DEG_FLIP: dest[X] = swap[X]; dest[Y] = -swap[Y]; dest[Z] = -swap[Z]; break; case CW270_DEG_FLIP: dest[X] = -swap[Y]; dest[Y] = -swap[X]; dest[Z] = -swap[Z]; break; } #ifndef SKIP_BOARD_ALIGNMENT if (!standardBoardAlignment) alignBoard(dest); #endif }
void alignSensors(int32_t *dest, uint8_t rotation) { const int32_t x = dest[X]; const int32_t y = dest[Y]; const int32_t z = dest[Z]; switch (rotation) { default: case CW0_DEG: dest[X] = x; dest[Y] = y; dest[Z] = z; break; case CW90_DEG: dest[X] = y; dest[Y] = -x; dest[Z] = z; break; case CW180_DEG: dest[X] = -x; dest[Y] = -y; dest[Z] = z; break; case CW270_DEG: dest[X] = -y; dest[Y] = x; dest[Z] = z; break; case CW0_DEG_FLIP: dest[X] = -x; dest[Y] = y; dest[Z] = -z; break; case CW90_DEG_FLIP: dest[X] = y; dest[Y] = x; dest[Z] = -z; break; case CW180_DEG_FLIP: dest[X] = x; dest[Y] = -y; dest[Z] = -z; break; case CW270_DEG_FLIP: dest[X] = -y; dest[Y] = -x; dest[Z] = -z; break; } if (!standardBoardAlignment) alignBoard(dest); }
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation) { switch (rotation) { case CW0_DEG: dest[X] = src[X]; dest[Y] = src[Y]; dest[Z] = src[Z]; break; case CW90_DEG: dest[X] = src[Y]; dest[Y] = -src[X]; dest[Z] = src[Z]; break; case CW180_DEG: dest[X] = -src[X]; dest[Y] = -src[Y]; dest[Z] = src[Z]; break; case CW270_DEG: dest[X] = -src[Y]; dest[Y] = src[X]; dest[Z] = src[Z]; break; case CW0_DEG_FLIP: dest[X] = -src[X]; dest[Y] = src[Y]; dest[Z] = -src[Z]; break; case CW90_DEG_FLIP: dest[X] = src[Y]; dest[Y] = src[X]; dest[Z] = -src[Z]; break; case CW180_DEG_FLIP: dest[X] = src[X]; dest[Y] = -src[Y]; dest[Z] = -src[Z]; break; case CW270_DEG_FLIP: dest[X] = -src[Y]; dest[Y] = -src[X]; dest[Z] = -src[Z]; break; default: break; } if (!standardBoardAlignment) alignBoard(dest); }