motion-module.cpp
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 #include <mutex>
5 #define _USE_MATH_DEFINES
6 #include <math.h>
7 
8 #include "hw-monitor.h"
9 #include "motion-module.h"
10 
11 using namespace rsimpl;
12 using namespace motion_module;
13 
15 const double IMU_UNITS_TO_MSEC = 0.00003125;
16 
17 motion_module_control::motion_module_control(uvc::device *device, std::timed_mutex& usbMutex) : device_handle(device), usbMutex(usbMutex), power_state(false)
18 {
19 }
20 
22 {
23  int tmp = state;
24  tmp += (int)request * (on ? 1 : -1);
25 
26  return tmp;
27 }
28 
30 {
31  std::lock_guard<std::mutex> lock(mtx);
32 
33  auto new_state = state_handler.requested_state(request, on);
34 
35  if (motion_module_state::valid(new_state))
36  enter_state((mm_state)new_state);
37  else
38  throw std::logic_error(to_string() << "MM invalid mode from" << state_handler.state << " to " << new_state);
39 }
40 
42 {
43  if (new_state == state_handler.state)
44  return;
45 
46  switch (state_handler.state)
47  {
48  case mm_idle:
49  if (mm_streaming == new_state)
50  {
51  // Power off before power on- Ensure that we starting from scratch
55  }
56  if (mm_eventing == new_state)
57  {
58  // Power off before power on- Ensure that we starting from scratch
61  set_control(mm_video_output, true); // L -shape adapter board
62  std::this_thread::sleep_for(std::chrono::milliseconds(300)); // Added delay between MM power on and MM start commands to be sure that the MM will be ready until start polling events.
64  }
65  break;
66  case mm_streaming:
67  if (mm_idle == new_state)
68  {
71  }
72  if (mm_full_load == new_state)
73  {
74  std::this_thread::sleep_for(std::chrono::milliseconds(300)); // Added delay between MM power on and MM start commands to be sure that the MM will be ready until start polling events.
76  }
77  if (mm_eventing == new_state)
78  {
79  throw std::logic_error(" Invalid Motion Module transition from streaming to motion tracking");
80  }
81  break;
82  case mm_eventing:
83  if (mm_idle == new_state)
84  {
86  set_control(mm_video_output, false); //Prevent power down
87  }
88  if (mm_full_load == new_state)
89  {
91  }
92  if (mm_streaming == new_state)
93  {
94  throw std::logic_error(" Invalid Motion Module transition from motion tracking to streaming");
95  }
96  break;
97  case mm_full_load:
98  if (mm_streaming == new_state)
99  {
101  }
102  if (mm_idle == new_state)
103  {
106  throw std::logic_error(" Invalid Motion Module transition from full to idle");
107  }
108  break;
109  default:
110  break;
111  }
112 
113  state_handler.state = new_state;
114 }
115 
117 {
118  adaptor_board_command cmd_opcode;
119  switch (request)
120  {
121  case mm_video_output:
122  cmd_opcode = adaptor_board_command::MMPWR;
123  break;
124  case mm_events_output:
126  break;
127  default:
128  throw std::logic_error(to_string() << " unsupported control requested :" << (int)request << " valid range is [1,2]");
129  }
130 
131  hw_monitor::hwmon_cmd cmd((uint8_t)cmd_opcode);
132  cmd.Param1 = (on) ? 1 : 0;
133 
134  // Motion module will always use the auxiliary USB handle (1) for
136 }
137 
139 {
140  if (power_state != on) // prevent re-entrance
141  {
142  // Apply user request, and update motion module controls if needed
143  impose(mm_video_output, on);
144  power_state = on;
145  }
146 }
147 
149 {
150  // Apply user request, and update motion module controls if needed
152 }
153 
154 
155 // Write a buffer to the IAP I2C register.
156 void motion_module_control::i2c_iap_write(uint16_t slave_address, uint8_t *buffer, uint16_t len)
157 {
159 
160  cmd.Param1 = slave_address;
161  cmd.Param2 = len;
162 
164  memcpy(cmd.data, buffer, len);
165 
167 }
168 
169 // Write a 32 bit value to a specific i2c slave address.
170 
171 
172 // switch the mtion module to IAP mode.
174 {
175  uint32_t value = -1;
176 
177  // read state.
178  hw_monitor::i2c_read_reg(static_cast<int>(adaptor_board_command::IRB), *device_handle, MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (int)i2c_register::REG_CURR_PWR_STATE, sizeof(uint32_t), reinterpret_cast<byte*>(&value));
179 
181  // we are not in IAP. switch to IAP.
183  }
184 
185  // retry for 10 times to be in IAP state.
186  for (int retries = 0; retries < 10; retries++)
187  {
188  std::this_thread::sleep_for(std::chrono::milliseconds(100));
189 
190  hw_monitor::i2c_read_reg(static_cast<int>(adaptor_board_command::IRB), *device_handle, MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (int)i2c_register::REG_CURR_PWR_STATE, sizeof(uint32_t), reinterpret_cast<byte*>(&value));
192  break; // we have entered IAP
193  }
194 
196  throw std::runtime_error("Unable to enter IAP state!");
197 }
198 
200 {
201  uint32_t value = -1;
202 
203  // write to the REG_JUMP_TO_APP register. this should return us to operational mode if all went well.
205 
206  hw_monitor::i2c_read_reg(static_cast<int>(adaptor_board_command::IRB), *device_handle, MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (int)i2c_register::REG_CURR_PWR_STATE, sizeof(uint32_t), reinterpret_cast<byte*>(&value));
207 
209  throw std::runtime_error("Unable to leave IAP state!");
210 }
211 
212 // Write the firmware.
214 {
215  int32_t length = size;
216  uint32_t image_address = 0x8002000;
217  fw_image_packet packet;
218  uint8_t *data_buffer = data;
219 
220  // we considered to be in a IAP state here. we loop through the data and send it in packets of 128 bytes.
221  while(length > 0)
222  {
223  uint16_t payload_length = (length > FW_IMAGE_PACKET_PAYLOAD_LEN) ? FW_IMAGE_PACKET_PAYLOAD_LEN : length;
224  uint32_t image_address_be;
225  uint16_t payload_be;
226 
227  // TODO SERGEY : The firmware needs the image address and payload length as big endian. do we have any
228  // little to big endian functions in this code ? we should detect that the machine is little endian and only
229  // then do something.
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;
234 
235  payload_be = (payload_length >> 8) & 0xFF;
236  payload_be |= (payload_length << 8) & 0xFF00;
237 
238  // packet require op_code of 0x6. can't find documentation for that.
239  packet.address = image_address_be;
240  packet.op_code = 0x6;
241  packet.length = payload_be;
242  packet.dummy = 0;
243 
244  // calcuate packet Length which includes the packet size and payload.
245  uint16_t packetLength = (sizeof(packet) - FW_IMAGE_PACKET_PAYLOAD_LEN + payload_length);
246 
247  // copy data to packet.
248  memcpy(packet.data, data_buffer, payload_length);
249 
250  // write to IAP.
251  i2c_iap_write(MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (uint8_t *)&packet, packetLength);
252 
253  // go to next packet if needed.
254  data_buffer += payload_length;
255  length -= payload_length;
256  image_address += payload_length;
257  };
258 }
259 
260 // This function responsible for the whole firmware upgrade process.
262 {
264  // power on motion mmodule (if needed).
266 
267  // swtich to IAP if needed.
268  switch_to_iap();
269 
270  //write the firmware.
271  write_firmware((uint8_t *)data, size);
272 
273  // write to operational mode.
275 }
276 
277 void motion_module::config(uvc::device&, uint8_t, uint8_t, uint8_t, uint8_t, uint32_t)
278 {
279  throw std::runtime_error(to_string() << __FUNCTION__ << " is not implemented");
280 }
281 
282 
283 std::vector<motion_event> motion_module_parser::operator() (const unsigned char* data, const int& data_size)
284 {
285  /* All sizes are in bytes*/
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; /* IMU SaS spec 3.3.2 */
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;
294 
295  std::vector<motion_event> v;
296 
297  if (packets)
298  {
299  unsigned char *cur_packet = nullptr;
300 
301  for (uint8_t i = 0; i < packets; i++)
302  {
303  motion_event event_data = {};
304 
305  cur_packet = (unsigned char*)data + (i*motion_packet_size);
306 
307  // extract packet header
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));
311  memcpy(&event_data.non_imu_entries_num, &cur_packet[6], sizeof(unsigned short));
312 
313  if (event_data.error_state.any())
314  {
315  LOG_WARNING("Motion Event: packet-level error detected " << event_data.error_state.to_string() << " packet will be dropped");
316  break;
317  }
318 
319  // Validate header input
320  if ((event_data.imu_entries_num <= imu_data_entries) && (event_data.non_imu_entries_num <= non_imu_data_entries))
321  {
322  // Parse IMU entries
323  for (uint8_t j = 0; j < event_data.imu_entries_num; j++)
324  {
325  event_data.imu_packets[j] = parse_motion(&cur_packet[motion_packet_header_size + j*imu_entry_size]);
326  }
327 
328  // Parse non-IMU entries
329  for (uint8_t j = 0; j < event_data.non_imu_entries_num; j++)
330  {
331  parse_timestamp(&cur_packet[non_imu_data_offset + j*non_imu_entry_size], event_data.non_imu_packets[j]);
332  }
333 
334  v.push_back(std::move(event_data));
335  }
336  }
337  }
338 
339  return v;
340 }
341 
343 {
344  // assuming msb ordering
345  unsigned short tmp = (data[1] << 8) | (data[0]);
346 
347  entry.source_id = rs_event_source((tmp & 0x7) - 1); // bits [0:2] - source_id
348  entry.frame_number = mm_data_wraparound[entry.source_id].frame_counter_wraparound.fix((tmp & 0x7fff) >> 3); // bits [3-14] - frame num
349  unsigned int timestamp;
350  memcpy(&timestamp, &data[2], sizeof(unsigned int)); // bits [16:47] - timestamp
351  entry.timestamp = mm_data_wraparound[entry.source_id].timestamp_wraparound.fix(timestamp) * IMU_UNITS_TO_MSEC; // Convert ticks to ms
352 }
353 
355 {
356  // predefined motion devices parameters
357  const static float gravity = 9.80665f; // Standard Gravitation Acceleration
358  const static float gyro_range = 1000.f; // Preconfigured angular velocity range [-1000...1000] Deg_C/Sec
359  const static float gyro_transform_factor = float((gyro_range * M_PI) / (180.f * 32767.f));
360 
361  const static float accel_range = 4.f; // Accelerometer is preset to [-4...+4]g range
362  const static float accelerator_transform_factor = float(gravity * accel_range / 2048.f);
363 
364  rs_motion_data entry;
365 
366  parse_timestamp(data, (rs_timestamp_data&)entry);
367 
368  entry.is_valid = (data[1] >> 7); // Isolate bit[15]
369 
370  // Read the motion tracking data for the three measured axes
371  short tmp[3];
372  memcpy(&tmp, &data[6], sizeof(short) * 3);
373 
374  unsigned data_shift = (RS_EVENT_IMU_ACCEL == entry.timestamp_data.source_id) ? 4 : 0; // Acceleration data is stored in 12 MSB
375 
376  for (int i = 0; i < 3; i++) // convert axis data to physical units, (m/sec^2) or (rad/sec)
377  {
378  entry.axes[i] = float(tmp[i] >> data_shift);
379  if (RS_EVENT_IMU_ACCEL == entry.timestamp_data.source_id) entry.axes[i] *= accelerator_transform_factor;
380  if (RS_EVENT_IMU_GYRO == entry.timestamp_data.source_id) entry.axes[i] *= gyro_transform_factor;
381 
382  // TODO check and report invalid conversion requests
383  }
384 
385  return entry;
386 }
motion_module_control(uvc::device *device, std::timed_mutex &usbMutex)
rs_event_source source_id
Definition: rs.h:342
const uint8_t MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS
rs_motion_data parse_motion(const unsigned char *data)
#define LOG_WARNING(...)
Definition: types.h:79
void write_firmware(uint8_t *data, int size)
uint8_t data[HW_MONITOR_BUFFER_SIZE]
Definition: hw-monitor.h:71
void impose(mm_request request, bool on)
Definition: archive.h:12
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)
GLuint buffer
Definition: glext.h:528
int requested_state(mm_request, bool on) const
void perform_and_send_monitor_command(uvc::device &device, std::timed_mutex &mutex, hwmon_cmd &newCommand)
Definition: hw-monitor.cpp:95
void parse_timestamp(const unsigned char *data, rs_timestamp_data &)
GLenum GLsizei len
Definition: glext.h:3213
double timestamp
Definition: rs.h:341
Motion data from gyroscope and accelerometer from the microcontroller.
Definition: rs.h:347
GLfloat f
Definition: glext.h:1868
static bool valid(int check_state)
typedef int(WINAPI *PFNWGLRELEASEPBUFFERDCARBPROC)(HPBUFFERARB hPbuffer
GLint GLenum GLsizei GLsizei GLsizei GLint GLsizei const void * data
Definition: glext.h:223
Timestamp data from the motion microcontroller.
Definition: rs.h:339
GLsizei const GLfloat * value
Definition: glext.h:693
#define FW_IMAGE_PACKET_PAYLOAD_LEN
Definition: motion-module.h:96
const GLdouble * v
Definition: glext.h:232
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)
Definition: hw-monitor.cpp:166
void set_control(mm_request request, bool on)
rs_timestamp_data non_imu_packets[8]
Definition: motion-module.h:83
GLsizeiptr size
Definition: glext.h:532
GLuint GLsizei GLsizei * length
Definition: glext.h:664
unsigned long long frame_number
Definition: rs.h:343
void i2c_read_reg(int command, uvc::device &device, uint16_t slave_address, uint16_t reg, uint32_t size, byte *data)
Definition: hw-monitor.cpp:184
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.
Definition: rs.h:276


librealsense
Author(s): Sergey Dorodnicov , Mark Horn , Reagan Lopez
autogenerated on Fri Mar 13 2020 03:16:17