Changeset 224 in flair-src for trunk/lib/FlairSensorActuator/src
- Timestamp:
- Mar 6, 2018, 12:12:58 PM (7 years ago)
- Location:
- trunk/lib/FlairSensorActuator/src
- Files:
-
- 12 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/lib/FlairSensorActuator/src/SimuBldc.cpp
r214 r224 50 50 } 51 51 52 SimuBldc::SimuBldc(const Object *parent, string name, uint8_t motors_count,53 uint32_t modelId,uint32_t deviceId)54 : Bldc(parent, name, motors_count) {55 shmem =56 new SharedMem(this, ShMemName(modelId, deviceId), motors_count * sizeof(float)+sizeof(Time));57 58 buf=(char*)malloc(motors_count * sizeof(float)+sizeof(Time));59 if(buf==NULL) {60 Err("error creating buffer\n");61 return;62 }63 64 // reset values65 float *values=(float*)buf;66 for (int i = 0; i < motors_count; i++) values[i] = 0;67 Time time=GetTime();68 memcpy(buf+motors_count * sizeof(float),&time,sizeof(Time));69 70 shmem->Write(buf, motors_count * sizeof(float)+sizeof(Time));71 72 SetIsReady(true);73 }74 75 52 SimuBldc::~SimuBldc() { 76 53 if(buf!=NULL) free(buf); … … 99 76 } 100 77 101 void SimuBldc::GetSpeeds(float *value,Time* time) const {102 float *values=(float*)buf;103 shmem->Read(buf, MotorsCount() * sizeof(float)+sizeof(Time));104 memcpy(time,buf+MotorsCount() * sizeof(float),sizeof(Time));105 106 for (int i = 0; i < MotorsCount(); i++) {107 value[i] = values[i];108 }109 }110 111 78 } // end namespace sensor 112 79 } // end namespace flair -
trunk/lib/FlairSensorActuator/src/SimuBldc.h
r214 r224 40 40 * \brief Constructor 41 41 * 42 * Construct a SimuBldc . Control part.42 * Construct a SimuBldc 43 43 * 44 44 * \param parent parent … … 53 53 54 54 /*! 55 * \brief Constructor56 *57 * Construct a SimuBldc. Simulation part.58 *59 * \param parent parent60 * \param name name61 * \param motors_count number of motors62 * \param modelId Model id63 * \param deviceId Bldc id of the Model64 */65 SimuBldc(const core::Object *parent, std::string name, uint8_t motors_count,66 uint32_t modelId,uint32_t deviceId);67 68 /*!69 55 * \brief Destructor 70 56 * 71 57 */ 72 58 ~SimuBldc(); 73 74 /*!75 * \brief Get motors speeds.76 *77 * This function should only be used for the simulation part.78 *79 * \param value array to store motors speeds80 * \param time time when shared memory was written81 */82 void GetSpeeds(float *value,core::Time* time) const;83 59 84 60 /*! -
trunk/lib/FlairSensorActuator/src/SimuCamera.cpp
r206 r224 30 30 namespace sensor { 31 31 32 //control part33 32 SimuCamera::SimuCamera(string name, 34 33 uint16_t width, uint16_t height, uint8_t channels, … … 49 48 } 50 49 51 //simulation part52 SimuCamera::SimuCamera(const IODevice *parent, string name, uint16_t width,53 uint16_t height, uint8_t channels, uint32_t modelId,uint32_t deviceId)54 : Thread(parent, name, 0), Camera(parent,name) {55 56 buf_size = width * height * channels+sizeof(Time);57 58 shmem = new SharedMem((Thread *)this,ShMemName(modelId, deviceId),59 buf_size, SharedMem::Type::producerConsumer);60 shmemReadBuf=NULL;61 62 SetIsReady(true);63 }64 50 65 51 SimuCamera::~SimuCamera() { -
trunk/lib/FlairSensorActuator/src/SimuCamera.h
r158 r224 35 35 * \brief Constructor 36 36 * 37 * Construct a SimuCamera. Control part.37 * Construct a SimuCamera. 38 38 * It will be child of the FrameworkManager. 39 39 * … … 50 50 uint8_t priority); 51 51 52 /*!53 * \brief Constructor54 *55 * Construct a SimuCamera. Simulation part.\n56 * The Thread of this class should not be run.57 *58 * \param parent parent59 * \param name name60 * \param width width61 * \param height height62 * \param channels number of channels63 * \param modelId Model id64 * \param deviceId Camera id of the Model65 */66 SimuCamera(const core::IODevice *parent, std::string name, uint16_t width,67 uint16_t height, uint8_t channels, uint32_t modelId,uint32_t deviceId);68 52 69 53 /*! -
trunk/lib/FlairSensorActuator/src/SimuGps.cpp
r214 r224 18 18 #include "SimuGps.h" 19 19 #include <FrameworkManager.h> 20 #include <GeoCoordinate.h>21 20 #include <GpsData.h> 22 21 #include <SharedMem.h> … … 57 56 58 57 59 SimuGps::SimuGps(const IODevice *parent, string name, uint32_t modelId,uint32_t deviceId)60 : NmeaGps(parent, name), Thread(parent, name, 0) {61 dataRate = NULL;62 63 shmem = new SharedMem((Thread *)this,ShMemName(modelId, deviceId),64 sizeof(gps_states_t));65 66 SetIsReady(true);67 }68 69 58 SimuGps::~SimuGps() { 70 59 SafeStop(); … … 76 65 dev_name << "simu" << modelId << "_gps_" << deviceId; 77 66 return dev_name.str().c_str(); 78 }79 80 void SimuGps::UpdateFrom(const io_data *data) {81 if (data != NULL) {82 Matrix *input = (Matrix *)data;83 gps_states_t state;84 85 input->GetMutex();86 //simulator is ned, convert it to enu87 //TODO: put simulator in enu?88 state.e = input->ValueNoMutex(5, 0);//y simulator89 state.n = input->ValueNoMutex(4, 0);//x simulator90 state.u = -input->ValueNoMutex(6, 0);//z simulator91 state.ve = input->ValueNoMutex(11, 0);//vy simulator92 state.vn = input->ValueNoMutex(10, 0);//vx simulator93 input->ReleaseMutex();94 95 shmem->Write((char *)&state, sizeof(gps_states_t));96 }97 67 } 98 68 -
trunk/lib/FlairSensorActuator/src/SimuGps.h
r206 r224 38 38 * \brief Constructor 39 39 * 40 * Construct a simulation GPS . Control part.40 * Construct a simulation GPS 41 41 * It will be child of the FrameworkManager. 42 42 * … … 49 49 SimuGps(std::string name, 50 50 NmeaGps::NMEAFlags_t NMEAFlags, uint32_t modelId,uint32_t deviceId,uint8_t priority); 51 52 /*!53 * \brief Constructor54 *55 * Construct a simulation GPS. Simulation part.\n56 * The Thread of this class should not be run.57 *58 * \param parent parent59 * \param name name60 * \param modelId Model id61 * \param deviceId Gps id of the Model62 */63 SimuGps(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);64 51 65 52 /*! … … 77 64 * \param data data from the parent to process 78 65 */ 79 void UpdateFrom(const core::io_data *data) ;66 void UpdateFrom(const core::io_data *data){}; 80 67 81 68 /*! -
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 -
trunk/lib/FlairSensorActuator/src/SimuImu.h
r186 r224 38 38 * \brief Constructor 39 39 * 40 * Construct a SimuImu. Control part.40 * Construct a SimuImu. 41 41 * It will be child of the FrameworkManager. 42 42 * … … 48 48 SimuImu(std::string name, 49 49 uint32_t modelId,uint32_t deviceId, uint8_t priority); 50 51 /*!52 * \brief Constructor53 *54 * Construct a SimuImu. Simulation part.\n55 * The Thread of this class should not be run.56 *57 * \param parent parent58 * \param name name59 * \param modelId Model id60 * \param deviceId Imu id of the Model61 */62 SimuImu(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);63 50 64 51 /*! … … 84 71 * \param data data from the parent to process 85 72 */ 86 void UpdateFrom(const core::io_data *data) ;73 void UpdateFrom(const core::io_data *data){}; 87 74 88 75 typedef struct { -
trunk/lib/FlairSensorActuator/src/SimuLaser.cpp
r214 r224 17 17 #include "SimuLaser.h" 18 18 #include <FrameworkManager.h> 19 #include <ImuData.h>20 19 #include <SpinBox.h> 21 20 #include <GroupBox.h> … … 37 36 data_rate = 38 37 new SpinBox(GetGroupBox()->NewRow(), "data rate", " Hz", 1, 500, 1, 50); 39 40 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),41 360 * sizeof(float));42 SetIsReady(true);43 }44 45 SimuLaser::SimuLaser(const IODevice *parent, string name, uint32_t modelId,uint32_t deviceId)46 : Thread(parent, name, 0), LaserRangeFinder(parent, name) {47 data_rate = NULL;48 38 49 39 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId), -
trunk/lib/FlairSensorActuator/src/SimuLaser.h
r158 r224 37 37 * \brief Constructor 38 38 * 39 * Construct a SimuUs. Control part. 40 * It will be child of the FrameworkManager. 39 * Construct a SimuUs. 41 40 * 42 41 * \param name name … … 47 46 SimuLaser(std::string name, 48 47 uint32_t modelId,uint32_t deviceId, uint8_t priority); 49 50 /*!51 * \brief Constructor52 *53 * Construct a SimuUs. Simulation part.\n54 * The Thread of this class should not be run.55 *56 * \param parent parent57 * \param name name58 * \param modelId Model id59 * \param deviceId LaserRangeFinder id of the Model60 */61 SimuLaser(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);62 48 63 49 /*! -
trunk/lib/FlairSensorActuator/src/SimuUs.cpp
r221 r224 17 17 #include "SimuUs.h" 18 18 #include <FrameworkManager.h> 19 #include <ImuData.h>20 19 #include <SpinBox.h> 21 20 #include <GroupBox.h> … … 41 40 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId), sizeof(float)); 42 41 43 SetIsReady(true);44 }45 46 //simulation part47 SimuUs::SimuUs(const IODevice *parent, string name,uint32_t modelId,uint32_t deviceId)48 : Thread(parent, name, 0), UsRangeFinder(parent,name) {49 data_rate = NULL;50 51 shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId), sizeof(float));52 53 42 SetIsReady(true); 54 43 } -
trunk/lib/FlairSensorActuator/src/SimuUs.h
r158 r224 37 37 * \brief Constructor 38 38 * 39 * Construct a SimuUs. Control part.39 * Construct a SimuUs. 40 40 * It will be child of the FrameworkManager. 41 41 * … … 47 47 SimuUs(std::string name, 48 48 uint32_t modelId,uint32_t deviceId, uint8_t priority); 49 50 /*!51 * \brief Constructor52 *53 * Construct a SimuUs. Simulation part.\n54 * The Thread of this class should not be run.55 *56 * \param parent parent57 * \param name name58 * \param modelId Model id59 * \param deviceId UsRangeFinder id of the Model60 */61 SimuUs(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);62 49 63 50 /*!
Note:
See TracChangeset
for help on using the changeset viewer.