Ignore:
Timestamp:
03/06/18 12:12:58 (4 years ago)
Author:
Sanahuja Guillaume
Message:

maj for armv5te

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/SimuImu.cpp

    r214 r224  
    3434namespace sensor {
    3535
    36 //Construct a SimuImu. Control part.
    3736SimuImu::SimuImu(string name, uint32_t modelId,uint32_t deviceId,
    3837                 uint8_t priority)
     
    4241  ahrsData = new AhrsData((Imu *)this);
    4342 
    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 
    5443  shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),
    5544                        sizeof(imu_states_t));
     
    6655  dev_name << "simu" <<  modelId << "_imu_" << deviceId;
    6756  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   }
    9357}
    9458
Note: See TracChangeset for help on using the changeset viewer.