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  {
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:
169  mm_state state;
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 
182  void toggle_motion_module_power(bool on);
183  void toggle_motion_module_events(bool on);
184 
185  void switch_to_iap();
186  void switch_to_operational();
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
rsimpl::motion_module::motion_event::status
motion_event_status status
Definition: motion-module.h:78
rsimpl::motion_module::adaptor_board_command::IWB
@ IWB
rsimpl::motion_module::mm_accel_bandwidth::accel_bw_125hz
@ accel_bw_125hz
rsimpl::motion_module::motion_module_wraparound::timestamp_wraparound
wraparound_mechanism< unsigned long long > timestamp_wraparound
Definition: motion-module.h:148
rsimpl::motion_module::i2c_register::REG_GYRO_BYPASS_CMD2
@ REG_GYRO_BYPASS_CMD2
rsimpl::motion_module::motion_event_status::pwr_mode_change_done
unsigned pwr_mode_change_done
Definition: motion-module.h:70
rsimpl::motion_module::motion_module_state::requested_state
int requested_state(mm_request, bool on) const
rsimpl::motion_module::uctrl_not_responding
@ uctrl_not_responding
Definition: motion-module.h:29
rsimpl::motion_module::mm_accel_range::accel_range_4g
@ accel_range_4g
rsimpl::motion_module::motion_module_control
Definition: motion-module.h:176
rsimpl::motion_module::adaptor_board_command::GLD
@ GLD
rsimpl::motion_module::i2c_register::REG_GYRO_BW
@ REG_GYRO_BW
rsimpl::motion_module::power_states::PWR_STATE_ACTIVE
@ PWR_STATE_ACTIVE
rsimpl::motion_module::mm_accel_range::accel_range_16g
@ accel_range_16g
rsimpl::motion_module::motion_module_state::motion_module_state
motion_module_state()
Definition: motion-module.h:168
rsimpl::motion_module::i2c_register::REG_EEPROM_DATA
@ REG_EEPROM_DATA
rsimpl::motion_module::adaptor_board_command::MM_ACTIVATE
@ MM_ACTIVATE
rsimpl::motion_module::motion_module_wraparound
Definition: motion-module.h:143
rsimpl::motion_module::motion_event_status::cx3_packet_number
unsigned cx3_packet_number
Definition: motion-module.h:71
rsimpl::motion_module::motion_module_state::state
mm_state state
Definition: motion-module.h:168
rsimpl::motion_module::power_states::PWR_STATE_PAUSE
@ PWR_STATE_PAUSE
rsimpl::motion_module::motion_module_control::toggle_motion_module_power
void toggle_motion_module_power(bool on)
rsimpl::motion_module::motion_module_control::switch_to_iap
void switch_to_iap()
rsimpl::motion_module::missing_data_from_imu
@ missing_data_from_imu
Definition: motion-module.h:25
rsimpl::motion_module::mm_config::gyro_bandwidth
mm_gyro_bandwidth gyro_bandwidth
Definition: motion-module.h:91
rsimpl::motion_module::motion_module_wraparound::motion_module_wraparound
motion_module_wraparound()
Definition: motion-module.h:145
rsimpl::motion_module::i2c_register::REG_UCTRL_CFG
@ REG_UCTRL_CFG
rsimpl::motion_module::adaptor_board_command::IAP_IRB
@ IAP_IRB
rsimpl::motion_module::crc_error
@ crc_error
Definition: motion-module.h:24
rsimpl::motion_module::adaptor_board_command::FRCNT
@ FRCNT
rsimpl::motion_module::mm_config::gyro_range
mm_gyro_range gyro_range
Definition: motion-module.h:90
rsimpl::motion_module::mm_gyro_range::gyro_range_2000
@ gyro_range_2000
rsimpl::motion_module::i2c_register::REG_GYRO_BYPASS_RDOUT2
@ REG_GYRO_BYPASS_RDOUT2
rsimpl::motion_module::watchdog_reset
@ watchdog_reset
Definition: motion-module.h:23
rsimpl::motion_module::mm_streaming
@ mm_streaming
Definition: motion-module.h:128
rsimpl::motion_module::mm_accel_range::accel_range_default
@ accel_range_default
rsimpl::motion_module::i2c_register::REG_TIMER_RESET_VALUE
@ REG_TIMER_RESET_VALUE
rsimpl::motion_module::i2c_register::REG_IMG_VER
@ REG_IMG_VER
rsimpl::motion_module::motion_event::error_state
std::bitset< 16 > error_state
Definition: motion-module.h:77
rsimpl::motion_module::power_states::PWR_STATE_DNR
@ PWR_STATE_DNR
rsimpl::motion_module::i2c_register::REG_GYRO_BYPASS_CMD1
@ REG_GYRO_BYPASS_CMD1
rsimpl::motion_module::eeprom_error
@ eeprom_error
Definition: motion-module.h:27
rsimpl::motion_module::i2c_register::REG_NEXT_PWR_STATE
@ REG_NEXT_PWR_STATE
RS_EVENT_SOURCE_COUNT
@ RS_EVENT_SOURCE_COUNT
Definition: rs.h:285
rsimpl::motion_module::i2c_register::REG_ACC_BYPASS_CMD1
@ REG_ACC_BYPASS_CMD1
rsimpl::motion_module::i2c_register::REG_INT_ENABLE
@ REG_INT_ENABLE
rsimpl::motion_module::mm_config::accel_range
mm_accel_range accel_range
Definition: motion-module.h:92
device.h
rsimpl::motion_module::adaptor_board_command::MM_TRB
@ MM_TRB
rs_motion_data
Motion data from gyroscope and accelerometer from the microcontroller.
Definition: rs.h:347
rsimpl::motion_module::motion_module_control::switch_to_operational
void switch_to_operational()
rsimpl::motion_module::i2c_register::REG_EEPROM_CMD
@ REG_EEPROM_CMD
rsimpl::motion_module::power_states::PWR_STATE_IAP
@ PWR_STATE_IAP
rsimpl::motion_module::mm_config::accel_bandwidth
mm_accel_bandwidth accel_bandwidth
Definition: motion-module.h:93
rsimpl::motion_module::fw_image_packet::length
uint16_t length
Definition: motion-module.h:101
rsimpl::motion_module::fw_image_packet::data
uint8_t data[FW_IMAGE_PACKET_PAYLOAD_LEN]
Definition: motion-module.h:103
rsimpl::motion_module::mm_accel_bandwidth
mm_accel_bandwidth
Definition: motion-module.h:60
rsimpl::motion_module::i2c_register::REG_INT_TYPE_CONFIG
@ REG_INT_TYPE_CONFIG
rsimpl::motion_module::i2c_register::REG_ACC_BW
@ REG_ACC_BW
rsimpl::motion_module::i2c_register::REG_GYRO_RANGE
@ REG_GYRO_RANGE
rsimpl::motion_module::motion_event::non_imu_packets
rs_timestamp_data non_imu_packets[8]
Definition: motion-module.h:83
rsimpl::motion_module::general_cx3_failure
@ general_cx3_failure
Definition: motion-module.h:31
rsimpl::motion_module::get_mm_state_name
const char * get_mm_state_name(mm_state state)
Definition: motion-module.h:133
rsimpl::motion_module::motion_module_errors
motion_module_errors
Definition: motion-module.h:17
rsimpl::motion_module::max_mm_error
@ max_mm_error
Definition: motion-module.h:35
rsimpl::to_string
Definition: types.h:44
rsimpl::motion_module::fw_image_packet
Definition: motion-module.h:98
rsimpl::motion_module::adaptor_board_command::GPR
@ GPR
rsimpl::motion_module::i2c_register::REG_ACC_BYPASS_CMD2
@ REG_ACC_BYPASS_CMD2
rsimpl::motion_module::mm_accel_range
mm_accel_range
Definition: motion-module.h:52
rsimpl::motion_module::mm_eventing
@ mm_eventing
Definition: motion-module.h:129
rsimpl::motion_module::mm_output_undefined
@ mm_output_undefined
Definition: motion-module.h:111
rsimpl::motion_module::motion_event::non_imu_entries_num
unsigned short non_imu_entries_num
Definition: motion-module.h:80
rsimpl
Definition: archive.h:12
rsimpl::motion_module::motion_module_parser::parse_motion
rs_motion_data parse_motion(const unsigned char *data)
rsimpl::motion_module::i2c_register
i2c_register
Definition: motion-module.h:208
rsimpl::wraparound_mechanism< unsigned long long >
rsimpl::motion_module::motion_event
Definition: motion-module.h:75
rsimpl::motion_module::i2c_register::REG_ACC_RANGE
@ REG_ACC_RANGE
rsimpl::motion_module::get_mm_request_name
const char * get_mm_request_name(mm_request request)
Definition: motion-module.h:116
rsimpl::motion_module::reserved_1
@ reserved_1
Definition: motion-module.h:33
rsimpl::motion_module::i2c_register::REG_ERROR
@ REG_ERROR
rsimpl::motion_module::imu_not_responding
@ imu_not_responding
Definition: motion-module.h:19
rsimpl::motion_module::i2c_register::REG_ACC_BYPASS_RDOUT2
@ REG_ACC_BYPASS_RDOUT2
rsimpl::motion_module::motion_module_parser::motion_module_parser
motion_module_parser()
Definition: motion-module.h:154
rsimpl::motion_module::bist_check_failed
@ bist_check_failed
Definition: motion-module.h:28
rsimpl::motion_module::mm_request
mm_request
Definition: motion-module.h:109
rsimpl::motion_module::adaptor_board_command::MM_SNB
@ MM_SNB
rsimpl::motion_module::adaptor_board_command::GPW
@ GPW
rsimpl::motion_module::i2c_register::REG_STATUS_REG
@ REG_STATUS_REG
rsimpl::motion_module::i2c_register::REG_REQ_LTR
@ REG_REQ_LTR
rsimpl::motion_module::motion_event::imu_packets
rs_motion_data imu_packets[4]
Definition: motion-module.h:82
rsimpl::motion_module::mm_state
mm_state
Definition: motion-module.h:125
rsimpl::motion_module::i2c_register::REG_JUMP_TO_APP
@ REG_JUMP_TO_APP
rsimpl::motion_module::mm_config::mm_time_seed
uint32_t mm_time_seed
Definition: motion-module.h:89
rsimpl::motion_module::illegal_configuration
@ illegal_configuration
Definition: motion-module.h:20
rsimpl::motion_module::mm_gyro_range::gyro_range_default
@ gyro_range_default
rsimpl::motion_module::mm_full_load
@ mm_full_load
Definition: motion-module.h:130
rsimpl::motion_module::mm_config
Definition: motion-module.h:87
rsimpl::motion_module::motion_module_parser::parse_timestamp
void parse_timestamp(const unsigned char *data, rs_timestamp_data &)
rsimpl::motion_module::adaptor_board_command::GVD
@ GVD
rsimpl::motion_module::motion_event_status::reserved_0
unsigned reserved_0
Definition: motion-module.h:69
FW_IMAGE_PACKET_PAYLOAD_LEN
#define FW_IMAGE_PACKET_PAYLOAD_LEN
Definition: motion-module.h:96
rsimpl::motion_module::mm_events_output
@ mm_events_output
Definition: motion-module.h:113
rsimpl::motion_module::mm_gyro_range
mm_gyro_range
Definition: motion-module.h:39
rsimpl::motion_module::cx3_fifo_overflow
@ cx3_fifo_overflow
Definition: motion-module.h:30
rsimpl::motion_module::fw_image_packet::address
uint32_t address
Definition: motion-module.h:100
rsimpl::motion_module::mm_gyro_range::gyro_range_1000
@ gyro_range_1000
rs_timestamp_data
Timestamp data from the motion microcontroller.
Definition: rs.h:339
rsimpl::motion_module::motion_event::imu_entries_num
unsigned short imu_entries_num
Definition: motion-module.h:79
rsimpl::motion_module::power_states
power_states
Definition: motion-module.h:241
rsimpl::motion_module::motion_module_wraparound::frame_counter_wraparound
wraparound_mechanism< unsigned long long > frame_counter_wraparound
Definition: motion-module.h:149
rsimpl::motion_module::i2c_register::REG_ACC_BYPASS_RDOUT1
@ REG_ACC_BYPASS_RDOUT1
rsimpl::motion_module::i2c_register::REG_FIFO_RD
@ REG_FIFO_RD
rsimpl::motion_module::ts_fifo_overflow
@ ts_fifo_overflow
Definition: motion-module.h:21
rsimpl::motion_module::mm_accel_range::accel_range_8g
@ accel_range_8g
rsimpl::motion_module::adaptor_board_command::MMPWR
@ MMPWR
rsimpl::motion_module::mm_accel_bandwidth::accel_bw_250hz
@ accel_bw_250hz
rsimpl::motion_module::mm_video_output
@ mm_video_output
Definition: motion-module.h:112
rsimpl::motion_module::motion_event::timestamp
unsigned long timestamp
Definition: motion-module.h:81
rsimpl::motion_module::motion_module_control::motion_module_control
motion_module_control(uvc::device *device, std::timed_mutex &usbMutex)
rsimpl::motion_module::adaptor_board_command::DSPWR
@ DSPWR
rsimpl::motion_module::motion_module_state::valid
static bool valid(int check_state)
Definition: motion-module.h:171
rsimpl::motion_module::adaptor_board_command::EXT_TRIG
@ EXT_TRIG
rsimpl::motion_module::motion_module_parser::operator()
std::vector< motion_event > operator()(const unsigned char *data, const int &data_size)
rsimpl::motion_module::config
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)
rsimpl::motion_module::cx3_usb_failure
@ cx3_usb_failure
Definition: motion-module.h:32
rsimpl::motion_module::adaptor_board_command
adaptor_board_command
Definition: motion-module.h:249
rsimpl::motion_module::adaptor_board_command::IAP_IWB
@ IAP_IWB
rsimpl::motion_module::motion_event_status::reserved_6_15
unsigned reserved_6_15
Definition: motion-module.h:72
rsimpl::motion_module::i2c_register::REG_INT_ID_CONFIG
@ REG_INT_ID_CONFIG
rsimpl::motion_module::mm_gyro_bandwidth::gyro_bw_default
@ gyro_bw_default
rsimpl::motion_module::motion_event_status
Definition: motion-module.h:67
rsimpl::motion_module::adaptor_board_command::CX3FWUPD
@ CX3FWUPD
rsimpl::motion_module::i2c_register::REG_FIRST_INT_IGNORE
@ REG_FIRST_INT_IGNORE
rsimpl::motion_module::mm_accel_bandwidth::accel_bw_default
@ accel_bw_default
rsimpl::motion_module::mm_gyro_bandwidth::gyro_bw_200hz
@ gyro_bw_200hz
rsimpl::motion_module::i2c_register::REG_TIMER_PRESCALER
@ REG_TIMER_PRESCALER
rsimpl::motion_module::adaptor_board_command::IRB
@ IRB
rsimpl::motion_module::imu_fifo_overflow
@ imu_fifo_overflow
Definition: motion-module.h:22
rsimpl::motion_module::mm_gyro_bandwidth
mm_gyro_bandwidth
Definition: motion-module.h:46
rsimpl::motion_module::fw_image_packet::dummy
uint8_t dummy
Definition: motion-module.h:102
rsimpl::motion_module::bootloader_failure
@ bootloader_failure
Definition: motion-module.h:26
rsimpl::motion_module::mm_idle
@ mm_idle
Definition: motion-module.h:127
rsimpl::motion_module::fw_image_packet::op_code
uint8_t op_code
Definition: motion-module.h:99
rsimpl::motion_module::i2c_register::REG_GYRO_BYPASS_RDOUT1
@ REG_GYRO_BYPASS_RDOUT1
rsimpl::motion_module::power_states::PWR_STATE_INIT
@ PWR_STATE_INIT
rsimpl::motion_module::reserved_2
@ reserved_2
Definition: motion-module.h:34
rsimpl::motion_module::motion_module_state
Definition: motion-module.h:165
rsimpl::motion_module::motion_module_parser::mm_data_wraparound
std::vector< motion_module_wraparound > mm_data_wraparound
Definition: motion-module.h:162
rsimpl::motion_module::i2c_register::REG_CURR_PWR_STATE
@ REG_CURR_PWR_STATE
rsimpl::motion_module::motion_module_control::firmware_upgrade
void firmware_upgrade(void *data, int size)
rsimpl::motion_module::motion_module_parser
Definition: motion-module.h:152
rsimpl::motion_module::motion_module_control::toggle_motion_module_events
void toggle_motion_module_events(bool on)
rsimpl::motion_module::i2c_register::REG_IAP_REG
@ REG_IAP_REG