Changeset 224 in flair-src for trunk/lib/FlairSensorActuator/src/SimuImu.cpp
- Timestamp:
- 03/06/18 12:12:58 (5 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/SimuImu.cpp
r214 r224 34 34 namespace sensor { 35 35 36 //Construct a SimuImu. Control part.37 36 SimuImu::SimuImu(string name, uint32_t modelId,uint32_t deviceId, 38 37 uint8_t priority) … … 42 41 ahrsData = new AhrsData((Imu *)this); 43 42 44 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),45 sizeof(imu_states_t));46 SetIsReady(true);47 }48 49 //Construct a SimuImu. Simulation part.50 SimuImu::SimuImu(const IODevice *parent, string name, uint32_t modelId,uint32_t deviceId)51 : Imu(parent,name), Thread(parent, name, 0) {52 dataRate = NULL;53 54 43 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId), 55 44 sizeof(imu_states_t)); … … 66 55 dev_name << "simu" << modelId << "_imu_" << deviceId; 67 56 return dev_name.str().c_str(); 68 }69 70 void SimuImu::UpdateFrom(const io_data *data) {71 if (data != NULL) {72 Matrix *input = (Matrix *)data;73 imu_states_t state;74 75 input->GetMutex();76 state.q0 = input->ValueNoMutex(0, 0);77 state.q1 = input->ValueNoMutex(1, 0);78 state.q2 = input->ValueNoMutex(2, 0);79 state.q3 = input->ValueNoMutex(3, 0);80 state.wx = input->ValueNoMutex(7, 0);81 state.wy = input->ValueNoMutex(8, 0);82 state.wz = input->ValueNoMutex(9, 0);83 state.ax = input->ValueNoMutex(13, 0);84 state.ay = input->ValueNoMutex(14, 0);85 state.az = input->ValueNoMutex(15, 0);86 state.mx = input->ValueNoMutex(16, 0);87 state.my = input->ValueNoMutex(17, 0);88 state.mz = input->ValueNoMutex(18, 0);89 input->ReleaseMutex();90 91 shmem->Write((char *)&state, sizeof(imu_states_t));92 }93 57 } 94 58
Note:
See TracChangeset
for help on using the changeset viewer.