00001
00002
00003
00004 #include <mutex>
00005 #define _USE_MATH_DEFINES
00006 #include <math.h>
00007
00008 #include "hw-monitor.h"
00009 #include "motion-module.h"
00010
00011 using namespace rsimpl;
00012 using namespace motion_module;
00013
00014 const uint8_t MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS = 0x42;
00015 const double IMU_UNITS_TO_MSEC = 0.00003125;
00016
00017 motion_module_control::motion_module_control(uvc::device *device, std::timed_mutex& usbMutex) : device_handle(device), usbMutex(usbMutex), power_state(false)
00018 {
00019 }
00020
00021 int motion_module_state::requested_state(mm_request request, bool on) const
00022 {
00023 int tmp = state;
00024 tmp += (int)request * (on ? 1 : -1);
00025
00026 return tmp;
00027 }
00028
00029 void motion_module_control::impose(mm_request request, bool on)
00030 {
00031 std::lock_guard<std::mutex> lock(mtx);
00032
00033 auto new_state = state_handler.requested_state(request, on);
00034
00035 if (motion_module_state::valid(new_state))
00036 enter_state((mm_state)new_state);
00037 else
00038 throw std::logic_error(to_string() << "MM invalid mode from" << state_handler.state << " to " << new_state);
00039 }
00040
00041 void motion_module_control::enter_state(mm_state new_state)
00042 {
00043 if (new_state == state_handler.state)
00044 return;
00045
00046 switch (state_handler.state)
00047 {
00048 case mm_idle:
00049 if (mm_streaming == new_state)
00050 {
00051
00052 set_control(mm_events_output, false);
00053 set_control(mm_video_output, false);
00054 set_control(mm_video_output, true);
00055 }
00056 if (mm_eventing == new_state)
00057 {
00058
00059 set_control(mm_events_output, false);
00060 set_control(mm_video_output, false);
00061 set_control(mm_video_output, true);
00062 std::this_thread::sleep_for(std::chrono::milliseconds(300));
00063 set_control(mm_events_output, true);
00064 }
00065 break;
00066 case mm_streaming:
00067 if (mm_idle == new_state)
00068 {
00069 set_control(mm_events_output, false);
00070 set_control(mm_video_output, false);
00071 }
00072 if (mm_full_load == new_state)
00073 {
00074 std::this_thread::sleep_for(std::chrono::milliseconds(300));
00075 set_control(mm_events_output, true);
00076 }
00077 if (mm_eventing == new_state)
00078 {
00079 throw std::logic_error(" Invalid Motion Module transition from streaming to motion tracking");
00080 }
00081 break;
00082 case mm_eventing:
00083 if (mm_idle == new_state)
00084 {
00085 set_control(mm_events_output, false);
00086 set_control(mm_video_output, false);
00087 }
00088 if (mm_full_load == new_state)
00089 {
00090 set_control(mm_events_output, true);
00091 }
00092 if (mm_streaming == new_state)
00093 {
00094 throw std::logic_error(" Invalid Motion Module transition from motion tracking to streaming");
00095 }
00096 break;
00097 case mm_full_load:
00098 if (mm_streaming == new_state)
00099 {
00100 set_control(mm_events_output, false);
00101 }
00102 if (mm_idle == new_state)
00103 {
00104 set_control(mm_events_output, false);
00105 set_control(mm_video_output, false);
00106 throw std::logic_error(" Invalid Motion Module transition from full to idle");
00107 }
00108 break;
00109 default:
00110 break;
00111 }
00112
00113 state_handler.state = new_state;
00114 }
00115
00116 void motion_module_control::set_control(mm_request request, bool on)
00117 {
00118 adaptor_board_command cmd_opcode;
00119 switch (request)
00120 {
00121 case mm_video_output:
00122 cmd_opcode = adaptor_board_command::MMPWR;
00123 break;
00124 case mm_events_output:
00125 cmd_opcode = adaptor_board_command::MM_ACTIVATE;
00126 break;
00127 default:
00128 throw std::logic_error(to_string() << " unsupported control requested :" << (int)request << " valid range is [1,2]");
00129 }
00130
00131 hw_monitor::hwmon_cmd cmd((uint8_t)cmd_opcode);
00132 cmd.Param1 = (on) ? 1 : 0;
00133
00134
00135 perform_and_send_monitor_command(*device_handle, usbMutex, cmd);
00136 }
00137
00138 void motion_module_control::toggle_motion_module_power(bool on)
00139 {
00140 if (power_state != on)
00141 {
00142
00143 impose(mm_video_output, on);
00144 power_state = on;
00145 }
00146 }
00147
00148 void motion_module_control::toggle_motion_module_events(bool on)
00149 {
00150
00151 impose(mm_events_output, on);
00152 }
00153
00154
00155
00156 void motion_module_control::i2c_iap_write(uint16_t slave_address, uint8_t *buffer, uint16_t len)
00157 {
00158 hw_monitor::hwmon_cmd cmd((int)adaptor_board_command::IAP_IWB);
00159
00160 cmd.Param1 = slave_address;
00161 cmd.Param2 = len;
00162
00163 cmd.sizeOfSendCommandData = len;
00164 memcpy(cmd.data, buffer, len);
00165
00166 perform_and_send_monitor_command(*device_handle, usbMutex, cmd);
00167 }
00168
00169
00170
00171
00172
00173 void motion_module_control::switch_to_iap()
00174 {
00175 uint32_t value = -1;
00176
00177
00178 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));
00179
00180 if ((power_states)value != power_states::PWR_STATE_IAP) {
00181
00182 hw_monitor::i2c_write_reg(static_cast<int>(adaptor_board_command::IWB), *device_handle, MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (int)i2c_register::REG_IAP_REG, 0xAE);
00183 }
00184
00185
00186 for (int retries = 0; retries < 10; retries++)
00187 {
00188 std::this_thread::sleep_for(std::chrono::milliseconds(100));
00189
00190 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));
00191 if ((power_states)value == power_states::PWR_STATE_IAP)
00192 break;
00193 }
00194
00195 if ((power_states)value != power_states::PWR_STATE_IAP)
00196 throw std::runtime_error("Unable to enter IAP state!");
00197 }
00198
00199 void motion_module_control::switch_to_operational()
00200 {
00201 uint32_t value = -1;
00202
00203
00204 hw_monitor::i2c_write_reg(static_cast<int>(adaptor_board_command::IWB), *device_handle, MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (int)i2c_register::REG_JUMP_TO_APP, 0x00);
00205
00206 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));
00207
00208 if ((power_states)value != power_states::PWR_STATE_INIT)
00209 throw std::runtime_error("Unable to leave IAP state!");
00210 }
00211
00212
00213 void motion_module_control::write_firmware(uint8_t *data, int size)
00214 {
00215 int32_t length = size;
00216 uint32_t image_address = 0x8002000;
00217 fw_image_packet packet;
00218 uint8_t *data_buffer = data;
00219
00220
00221 while(length > 0)
00222 {
00223 uint16_t payload_length = (length > FW_IMAGE_PACKET_PAYLOAD_LEN) ? FW_IMAGE_PACKET_PAYLOAD_LEN : length;
00224 uint32_t image_address_be;
00225 uint16_t payload_be;
00226
00227
00228
00229
00230 image_address_be = (image_address >> 24) & 0xFF;
00231 image_address_be |= (image_address >> 8) & 0xFF00;
00232 image_address_be |= (image_address << 8) & 0xFF0000;
00233 image_address_be |= (image_address << 24) & 0xFF000000;
00234
00235 payload_be = (payload_length >> 8) & 0xFF;
00236 payload_be |= (payload_length << 8) & 0xFF00;
00237
00238
00239 packet.address = image_address_be;
00240 packet.op_code = 0x6;
00241 packet.length = payload_be;
00242 packet.dummy = 0;
00243
00244
00245 uint16_t packetLength = (sizeof(packet) - FW_IMAGE_PACKET_PAYLOAD_LEN + payload_length);
00246
00247
00248 memcpy(packet.data, data_buffer, payload_length);
00249
00250
00251 i2c_iap_write(MOTION_MODULE_CONTROL_I2C_SLAVE_ADDRESS, (uint8_t *)&packet, packetLength);
00252
00253
00254 data_buffer += payload_length;
00255 length -= payload_length;
00256 image_address += payload_length;
00257 };
00258 }
00259
00260
00261 void motion_module_control::firmware_upgrade(void *data, int size)
00262 {
00263 set_control(mm_events_output, false);
00264
00265 toggle_motion_module_power(true);
00266
00267
00268 switch_to_iap();
00269
00270
00271 write_firmware((uint8_t *)data, size);
00272
00273
00274 switch_to_operational();
00275 }
00276
00277 void motion_module::config(uvc::device&, uint8_t, uint8_t, uint8_t, uint8_t, uint32_t)
00278 {
00279 throw std::runtime_error(to_string() << __FUNCTION__ << " is not implemented");
00280 }
00281
00282
00283 std::vector<motion_event> motion_module_parser::operator() (const unsigned char* data, const int& data_size)
00284 {
00285
00286 const unsigned short motion_packet_header_size = 8;
00287 const unsigned short imu_data_entries = 4;
00288 const unsigned short imu_entry_size = 12;
00289 const unsigned short non_imu_data_entries = 8;
00290 const unsigned short non_imu_entry_size = 6;
00291 const unsigned short non_imu_data_offset = motion_packet_header_size + (imu_data_entries * imu_entry_size);
00292 const unsigned short motion_packet_size = non_imu_data_offset + (non_imu_data_entries * non_imu_entry_size);
00293 unsigned short packets = data_size / motion_packet_size;
00294
00295 std::vector<motion_event> v;
00296
00297 if (packets)
00298 {
00299 unsigned char *cur_packet = nullptr;
00300
00301 for (uint8_t i = 0; i < packets; i++)
00302 {
00303 motion_event event_data = {};
00304
00305 cur_packet = (unsigned char*)data + (i*motion_packet_size);
00306
00307
00308 memcpy(&event_data.error_state, &cur_packet[0], sizeof(unsigned short));
00309 memcpy(&event_data.status, &cur_packet[2], sizeof(unsigned short));
00310 memcpy(&event_data.imu_entries_num, &cur_packet[4], sizeof(unsigned short));
00311 memcpy(&event_data.non_imu_entries_num, &cur_packet[6], sizeof(unsigned short));
00312
00313 if (event_data.error_state.any())
00314 {
00315 LOG_WARNING("Motion Event: packet-level error detected " << event_data.error_state.to_string() << " packet will be dropped");
00316 break;
00317 }
00318
00319
00320 if ((event_data.imu_entries_num <= imu_data_entries) && (event_data.non_imu_entries_num <= non_imu_data_entries))
00321 {
00322
00323 for (uint8_t j = 0; j < event_data.imu_entries_num; j++)
00324 {
00325 event_data.imu_packets[j] = parse_motion(&cur_packet[motion_packet_header_size + j*imu_entry_size]);
00326 }
00327
00328
00329 for (uint8_t j = 0; j < event_data.non_imu_entries_num; j++)
00330 {
00331 parse_timestamp(&cur_packet[non_imu_data_offset + j*non_imu_entry_size], event_data.non_imu_packets[j]);
00332 }
00333
00334 v.push_back(std::move(event_data));
00335 }
00336 }
00337 }
00338
00339 return v;
00340 }
00341
00342 void motion_module_parser::parse_timestamp(const unsigned char * data, rs_timestamp_data &entry)
00343 {
00344
00345 unsigned short tmp = (data[1] << 8) | (data[0]);
00346
00347 entry.source_id = rs_event_source((tmp & 0x7) - 1);
00348 entry.frame_number = mm_data_wraparound[entry.source_id].frame_counter_wraparound.fix((tmp & 0x7fff) >> 3);
00349 unsigned int timestamp;
00350 memcpy(×tamp, &data[2], sizeof(unsigned int));
00351 entry.timestamp = mm_data_wraparound[entry.source_id].timestamp_wraparound.fix(timestamp) * IMU_UNITS_TO_MSEC;
00352 }
00353
00354 rs_motion_data motion_module_parser::parse_motion(const unsigned char * data)
00355 {
00356
00357 const static float gravity = 9.80665f;
00358 const static float gyro_range = 1000.f;
00359 const static float gyro_transform_factor = float((gyro_range * M_PI) / (180.f * 32767.f));
00360
00361 const static float accel_range = 4.f;
00362 const static float accelerator_transform_factor = float(gravity * accel_range / 2048.f);
00363
00364 rs_motion_data entry;
00365
00366 parse_timestamp(data, (rs_timestamp_data&)entry);
00367
00368 entry.is_valid = (data[1] >> 7);
00369
00370
00371 short tmp[3];
00372 memcpy(&tmp, &data[6], sizeof(short) * 3);
00373
00374 unsigned data_shift = (RS_EVENT_IMU_ACCEL == entry.timestamp_data.source_id) ? 4 : 0;
00375
00376 for (int i = 0; i < 3; i++)
00377 {
00378 entry.axes[i] = float(tmp[i] >> data_shift);
00379 if (RS_EVENT_IMU_ACCEL == entry.timestamp_data.source_id) entry.axes[i] *= accelerator_transform_factor;
00380 if (RS_EVENT_IMU_GYRO == entry.timestamp_data.source_id) entry.axes[i] *= gyro_transform_factor;
00381
00382
00383 }
00384
00385 return entry;
00386 }