Intel® RealSense™ Cross Platform API
Intel Realsense Cross-platform API
motion-module.h
Go to the documentation of this file.
1 // License: Apache 2.0. See LICENSE file in root directory.
2 // Copyright(c) 2015 Intel Corporation. All Rights Reserved.
3 
4 #pragma once
5 #ifndef MOTION_MODULE_H
6 #define MOTION_MODULE_H
7 
8 #include <mutex>
9 #include <bitset>
10 
11 #include "device.h"
12 
13 namespace rsimpl
14 {
15  namespace motion_module
16  {
18  {
24  crc_error = 5,
33  reserved_1 = 14,
34  reserved_2 = 15,
36  };
37 
38  //RealSense DS41t SaS rev 0_60 specifications [deg/sec]
39  enum class mm_gyro_range : uint8_t
40  {
42  gyro_range_1000 = 1,
43  gyro_range_2000 = 2
44  };
45 
46  enum class mm_gyro_bandwidth : uint8_t
47  {
48  gyro_bw_default = 0,
49  gyro_bw_200hz = 1
50  };
51 
52  enum class mm_accel_range : uint8_t
53  {
55  accel_range_4g = 1,
56  accel_range_8g = 2,
57  accel_range_16g = 3
58  };
59 
60  enum class mm_accel_bandwidth : uint8_t
61  {
62  accel_bw_default = 0,
63  accel_bw_125hz = 1,
64  accel_bw_250hz = 2
65  };
66 
68  {
69  unsigned reserved_0 : 1;
70  unsigned pwr_mode_change_done : 1;
71  unsigned cx3_packet_number : 4; /* bits [2..5] packet counter number*/
72  unsigned reserved_6_15 : 10;
73  };
74 
75  struct motion_event
76  {
77  std::bitset<16> error_state;
79  unsigned short imu_entries_num;
80  unsigned short non_imu_entries_num;
81  unsigned long timestamp;
84  };
85 
86 #pragma pack(push, 1)
87  struct mm_config /* Motion module configuration data*/
88  {
89  uint32_t mm_time_seed;
94  };
95 
96  #define FW_IMAGE_PACKET_PAYLOAD_LEN (128)
97 
98  struct fw_image_packet {
99  uint8_t op_code;
100  uint32_t address;
101  uint16_t length;
102  uint8_t dummy;
104  };
105 #pragma pack(pop)
106 
107  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);
108 
109  enum mm_request : uint8_t
110  {
113  mm_events_output = 2
114  };
115 
116  inline const char* get_mm_request_name(mm_request request) {
117  switch (request) {
118  case mm_output_undefined: return "undefined";
119  case mm_video_output: return "video";
120  case mm_events_output: return "motion_tracking";
121  default: return std::string(to_string() << "unresolved request id: " << request).c_str();
122  }
123  }
124 
125  enum mm_state : uint8_t
126  {
127  mm_idle = 0, // Initial
128  mm_streaming = 1, // FishEye video only
129  mm_eventing = 2, // Motion data only
130  mm_full_load = 3 // Motion dat + FishEye streaming
131  };
132 
133  inline const char* get_mm_state_name(mm_state state) {
134  switch (state) {
135  case mm_idle: return "idle";
136  case mm_streaming: return "video";
137  case mm_eventing: return "motion";
138  case mm_full_load: return "video+motion";
139  default: return std::string(to_string() << "unresolved mm state id: " << state).c_str();
140  }
141  }
142 
144  {
146  : timestamp_wraparound(1, std::numeric_limits<uint32_t>::max()), frame_counter_wraparound(0, 0xfff)
147  {}
150  };
151 
153  {
156  {}
157 
158  std::vector<motion_event> operator() (const unsigned char* data, const int& data_size);
159  void parse_timestamp(const unsigned char* data, rs_timestamp_data &);
160  rs_motion_data parse_motion(const unsigned char* data);
161 
162  std::vector<motion_module_wraparound> mm_data_wraparound;
163  };
164 
166  {
167  public:
170  int requested_state(mm_request, bool on) const;
171  static bool valid(int check_state) { return ((check_state >= mm_idle) && (check_state <= mm_full_load)); }
172  private:
174  };
175 
177  {
178  public:
179 
180  motion_module_control(uvc::device *device, std::timed_mutex& usbMutex);
181 
184 
187 
188  void firmware_upgrade(void *data, int size);
189 
190  private:
192  motion_module_state state_handler;
193  uvc::device* device_handle;
194  std::mutex mtx;
195  std::timed_mutex& usbMutex;
196  bool power_state;
197 
198  void i2c_iap_write(uint16_t slave_address, uint8_t *buffer, uint16_t len);
199 
200  void write_firmware(uint8_t *data, int size);
201 
202  void impose(mm_request request, bool on);
203  void enter_state(mm_state new_state);
204  void set_control(mm_request request, bool on);
205 
206  };
207 
208  enum class i2c_register : uint32_t {
209  REG_UCTRL_CFG = 0x00,
210  REG_INT_ID_CONFIG = 0x04,
211  REG_INT_TYPE_CONFIG = 0x08,
212  REG_EEPROM_CMD = 0x0C,
213  REG_EEPROM_DATA = 0xD0,
214  REG_TIMER_PRESCALER = 0x14,
215  REG_TIMER_RESET_VALUE = 0x18,
216  REG_GYRO_BYPASS_CMD1 = 0x1C,
217  REG_GYRO_BYPASS_CMD2 = 0x20,
218  REG_GYRO_BYPASS_RDOUT1 = 0x24,
219  REG_GYRO_BYPASS_RDOUT2 = 0x28,
220  REG_ACC_BYPASS_CMD1 = 0x2C,
221  REG_ACC_BYPASS_CMD2 = 0x30,
222  REG_ACC_BYPASS_RDOUT1 = 0x34,
223  REG_ACC_BYPASS_RDOUT2 = 0x38,
224  REG_REQ_LTR = 0x3C,
225  REG_STATUS_REG = 0x40,
226  REG_FIFO_RD = 0x44,
227  REG_FIRST_INT_IGNORE = 0x48,
228  REG_IAP_REG = 0x4C,
229  REG_INT_ENABLE = 0x50,
230  REG_CURR_PWR_STATE = 0x54,
231  REG_NEXT_PWR_STATE = 0x58,
232  REG_ACC_BW = 0x5C,
233  REG_GYRO_BW = 0x60,
234  REG_ACC_RANGE = 0x64,
235  REG_GYRO_RANGE = 0x68,
236  REG_IMG_VER = 0x6C,
237  REG_ERROR = 0x70,
238  REG_JUMP_TO_APP = 0x77
239  };
240 
241  enum class power_states : uint32_t {
242  PWR_STATE_DNR = 0x00,
243  PWR_STATE_INIT = 0x02,
244  PWR_STATE_ACTIVE = 0x03,
245  PWR_STATE_PAUSE = 0x04,
246  PWR_STATE_IAP = 0x05
247  };
248 
249  enum class adaptor_board_command : uint32_t
250  {
251  IRB = 0x01, // Read from i2c ( 8x8 )
252  IWB = 0x02, // Write to i2c ( 8x8 )
253  GVD = 0x03, // Get Version and Date
254  IAP_IRB = 0x04, // Read from IAP i2c ( 8x8 )
255  IAP_IWB = 0x05, // Write to IAP i2c ( 8x8 )
256  FRCNT = 0x06, // Read frame counter
257  GLD = 0x07, // Get logger data
258  GPW = 0x08, // Write to GPIO
259  GPR = 0x09, // Read from GPIO
260  MMPWR = 0x0A, // Motion module power up/down
261  DSPWR = 0x0B, // DS4 power up/down
262  EXT_TRIG = 0x0C, // external trigger mode
263  CX3FWUPD = 0x0D, // FW update
264  MM_SNB = 0x10, // Get the serial number
265  MM_TRB = 0x11, // Get the extrinsics and intrinsics data
266  MM_ACTIVATE = 0x0E // Motion Module activation
267  };
268  } // namespace motion_module
269 }
270 
271 #endif // MOTION_MODULE_H
Definition: motion-module.h:177
void firmware_upgrade(void *data, int size)
motion_module_control(uvc::device *device, std::timed_mutex &usbMutex)
Definition: motion-module.h:166
int requested_state(mm_request, bool on) const
mm_state state
Definition: motion-module.h:168
static bool valid(int check_state)
Definition: motion-module.h:171
motion_module_state()
Definition: motion-module.h:168
#define FW_IMAGE_PACKET_PAYLOAD_LEN
Definition: motion-module.h:96
i2c_register
Definition: motion-module.h:208
adaptor_board_command
Definition: motion-module.h:250
mm_gyro_bandwidth
Definition: motion-module.h:47
const char * get_mm_request_name(mm_request request)
Definition: motion-module.h:116
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)
motion_module_errors
Definition: motion-module.h:18
@ eeprom_error
Definition: motion-module.h:27
@ illegal_configuration
Definition: motion-module.h:20
@ imu_fifo_overflow
Definition: motion-module.h:22
@ reserved_2
Definition: motion-module.h:34
@ uctrl_not_responding
Definition: motion-module.h:29
@ general_cx3_failure
Definition: motion-module.h:31
@ cx3_fifo_overflow
Definition: motion-module.h:30
@ bootloader_failure
Definition: motion-module.h:26
@ ts_fifo_overflow
Definition: motion-module.h:21
@ max_mm_error
Definition: motion-module.h:35
@ crc_error
Definition: motion-module.h:24
@ bist_check_failed
Definition: motion-module.h:28
@ missing_data_from_imu
Definition: motion-module.h:25
@ imu_not_responding
Definition: motion-module.h:19
@ cx3_usb_failure
Definition: motion-module.h:32
@ watchdog_reset
Definition: motion-module.h:23
@ reserved_1
Definition: motion-module.h:33
mm_state
Definition: motion-module.h:126
@ mm_full_load
Definition: motion-module.h:130
@ mm_eventing
Definition: motion-module.h:129
@ mm_idle
Definition: motion-module.h:127
@ mm_streaming
Definition: motion-module.h:128
mm_accel_range
Definition: motion-module.h:53
mm_gyro_range
Definition: motion-module.h:40
power_states
Definition: motion-module.h:241
const char * get_mm_state_name(mm_state state)
Definition: motion-module.h:133
mm_request
Definition: motion-module.h:110
@ mm_events_output
Definition: motion-module.h:113
@ mm_output_undefined
Definition: motion-module.h:111
@ mm_video_output
Definition: motion-module.h:112
mm_accel_bandwidth
Definition: motion-module.h:61
Definition: archive.h:13
@ RS_EVENT_SOURCE_COUNT
Definition: rs.h:285
Motion data from gyroscope and accelerometer from the microcontroller.
Definition: rs.h:348
Timestamp data from the motion microcontroller.
Definition: rs.h:340
Definition: motion-module.h:98
uint16_t length
Definition: motion-module.h:101
uint32_t address
Definition: motion-module.h:100
uint8_t op_code
Definition: motion-module.h:99
uint8_t data[FW_IMAGE_PACKET_PAYLOAD_LEN]
Definition: motion-module.h:103
uint8_t dummy
Definition: motion-module.h:102
Definition: motion-module.h:88
mm_accel_range accel_range
Definition: motion-module.h:92
mm_accel_bandwidth accel_bandwidth
Definition: motion-module.h:93
mm_gyro_range gyro_range
Definition: motion-module.h:90
mm_gyro_bandwidth gyro_bandwidth
Definition: motion-module.h:91
uint32_t mm_time_seed
Definition: motion-module.h:89
Definition: motion-module.h:68
unsigned reserved_0
Definition: motion-module.h:69
unsigned pwr_mode_change_done
Definition: motion-module.h:70
unsigned reserved_6_15
Definition: motion-module.h:72
unsigned cx3_packet_number
Definition: motion-module.h:71
Definition: motion-module.h:76
motion_event_status status
Definition: motion-module.h:78
rs_motion_data imu_packets[4]
Definition: motion-module.h:82
unsigned long timestamp
Definition: motion-module.h:81
rs_timestamp_data non_imu_packets[8]
Definition: motion-module.h:83
std::bitset< 16 > error_state
Definition: motion-module.h:77
unsigned short imu_entries_num
Definition: motion-module.h:79
unsigned short non_imu_entries_num
Definition: motion-module.h:80
Definition: motion-module.h:153
std::vector< motion_event > operator()(const unsigned char *data, const int &data_size)
std::vector< motion_module_wraparound > mm_data_wraparound
Definition: motion-module.h:162
void parse_timestamp(const unsigned char *data, rs_timestamp_data &)
motion_module_parser()
Definition: motion-module.h:154
rs_motion_data parse_motion(const unsigned char *data)
wraparound_mechanism< unsigned long long > timestamp_wraparound
Definition: motion-module.h:148
motion_module_wraparound()
Definition: motion-module.h:145
wraparound_mechanism< unsigned long long > frame_counter_wraparound
Definition: motion-module.h:149
Definition: types.h:45