5 #define _USE_MATH_DEFINES 12 using namespace motion_module;
24 tmp += (
int)request * (on ? 1 : -1);
31 std::lock_guard<std::mutex> lock(
mtx);
62 std::this_thread::sleep_for(std::chrono::milliseconds(300));
74 std::this_thread::sleep_for(std::chrono::milliseconds(300));
79 throw std::logic_error(
" Invalid Motion Module transition from streaming to motion tracking");
94 throw std::logic_error(
" Invalid Motion Module transition from motion tracking to streaming");
106 throw std::logic_error(
" Invalid Motion Module transition from full to idle");
128 throw std::logic_error(
to_string() <<
" unsupported control requested :" << (
int)request <<
" valid range is [1,2]");
132 cmd.
Param1 = (on) ? 1 : 0;
160 cmd.
Param1 = slave_address;
164 memcpy(cmd.
data, buffer, len);
186 for (
int retries = 0; retries < 10; retries++)
188 std::this_thread::sleep_for(std::chrono::milliseconds(100));
196 throw std::runtime_error(
"Unable to enter IAP state!");
209 throw std::runtime_error(
"Unable to leave IAP state!");
216 uint32_t image_address = 0x8002000;
218 uint8_t *data_buffer =
data;
224 uint32_t image_address_be;
230 image_address_be = (image_address >> 24) & 0xFF;
231 image_address_be |= (image_address >> 8) & 0xFF00;
232 image_address_be |= (image_address << 8) & 0xFF0000;
233 image_address_be |= (image_address << 24) & 0xFF000000;
235 payload_be = (payload_length >> 8) & 0xFF;
236 payload_be |= (payload_length << 8) & 0xFF00;
239 packet.
address = image_address_be;
241 packet.
length = payload_be;
248 memcpy(packet.
data, data_buffer, payload_length);
254 data_buffer += payload_length;
255 length -= payload_length;
256 image_address += payload_length;
279 throw std::runtime_error(
to_string() << __FUNCTION__ <<
" is not implemented");
286 const unsigned short motion_packet_header_size = 8;
287 const unsigned short imu_data_entries = 4;
288 const unsigned short imu_entry_size = 12;
289 const unsigned short non_imu_data_entries = 8;
290 const unsigned short non_imu_entry_size = 6;
291 const unsigned short non_imu_data_offset = motion_packet_header_size + (imu_data_entries * imu_entry_size);
292 const unsigned short motion_packet_size = non_imu_data_offset + (non_imu_data_entries * non_imu_entry_size);
293 unsigned short packets = data_size / motion_packet_size;
295 std::vector<motion_event>
v;
299 unsigned char *cur_packet =
nullptr;
301 for (uint8_t i = 0; i < packets; i++)
305 cur_packet = (
unsigned char*)data + (i*motion_packet_size);
308 memcpy(&event_data.
error_state, &cur_packet[0],
sizeof(
unsigned short));
309 memcpy(&event_data.
status, &cur_packet[2],
sizeof(
unsigned short));
310 memcpy(&event_data.
imu_entries_num, &cur_packet[4],
sizeof(
unsigned short));
315 LOG_WARNING(
"Motion Event: packet-level error detected " << event_data.
error_state.to_string() <<
" packet will be dropped");
325 event_data.
imu_packets[j] = parse_motion(&cur_packet[motion_packet_header_size + j*imu_entry_size]);
331 parse_timestamp(&cur_packet[non_imu_data_offset + j*non_imu_entry_size], event_data.
non_imu_packets[j]);
334 v.push_back(std::move(event_data));
345 unsigned short tmp = (data[1] << 8) | (data[0]);
348 entry.
frame_number = mm_data_wraparound[entry.
source_id].frame_counter_wraparound.fix((tmp & 0x7fff) >> 3);
349 unsigned int timestamp;
350 memcpy(×tamp, &data[2],
sizeof(
unsigned int));
357 const static float gravity = 9.80665f;
358 const static float gyro_range = 1000.f;
359 const static float gyro_transform_factor = float((gyro_range * M_PI) / (180.
f * 32767.
f));
361 const static float accel_range = 4.f;
362 const static float accelerator_transform_factor = float(gravity * accel_range / 2048.f);
368 entry.is_valid = (data[1] >> 7);
372 memcpy(&tmp, &data[6],
sizeof(
short) * 3);
376 for (
int i = 0; i < 3; i++)
378 entry.axes[i] = float(tmp[i] >> data_shift);
std::timed_mutex & usbMutex
motion_module_control(uvc::device *device, std::timed_mutex &usbMutex)
void enter_state(mm_state new_state)
rs_event_source source_id
unsigned short imu_entries_num
const uint8_t MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS
void toggle_motion_module_power(bool on)
rs_motion_data parse_motion(const unsigned char *data)
int sizeOfSendCommandData
std::bitset< 16 > error_state
unsigned short non_imu_entries_num
void write_firmware(uint8_t *data, int size)
uint8_t data[HW_MONITOR_BUFFER_SIZE]
void impose(mm_request request, bool on)
uvc::device * device_handle
void config(uvc::device &device, uint8_t gyro_bw, uint8_t gyro_range, uint8_t accel_bw, uint8_t accel_range, uint32_t time_seed)
void switch_to_operational()
int requested_state(mm_request, bool on) const
void perform_and_send_monitor_command(uvc::device &device, std::timed_mutex &mutex, hwmon_cmd &newCommand)
rs_motion_data imu_packets[4]
void parse_timestamp(const unsigned char *data, rs_timestamp_data &)
motion_event_status status
Motion data from gyroscope and accelerometer from the microcontroller.
static bool valid(int check_state)
motion_module_state state_handler
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
GLint GLenum GLsizei GLsizei GLsizei GLint GLsizei const void * data
void toggle_motion_module_events(bool on)
Timestamp data from the motion microcontroller.
GLsizei const GLfloat * value
#define FW_IMAGE_PACKET_PAYLOAD_LEN
const double IMU_UNITS_TO_MSEC
uint8_t data[FW_IMAGE_PACKET_PAYLOAD_LEN]
void i2c_write_reg(int command, uvc::device &device, uint16_t slave_address, uint16_t reg, uint32_t value)
void set_control(mm_request request, bool on)
rs_timestamp_data non_imu_packets[8]
GLuint GLsizei GLsizei * length
unsigned long long frame_number
void firmware_upgrade(void *data, int size)
void i2c_read_reg(int command, uvc::device &device, uint16_t slave_address, uint16_t reg, uint32_t size, byte *data)
std::vector< motion_event > operator()(const unsigned char *data, const int &data_size)
void i2c_iap_write(uint16_t slave_address, uint8_t *buffer, uint16_t len)
rs_event_source
Source device that triggered a specific timestamp event from the motion module.