Changeset 224 in flair-src for trunk/lib/FlairSensorActuator


Ignore:
Timestamp:
Mar 6, 2018, 12:12:58 PM (7 years ago)
Author:
Sanahuja Guillaume
Message:

maj for armv5te

Location:
trunk/lib/FlairSensorActuator/src
Files:
12 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairSensorActuator/src/SimuBldc.cpp

    r214 r224  
    5050}
    5151
    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 values
    65   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 
    7552SimuBldc::~SimuBldc() {
    7653  if(buf!=NULL) free(buf);
    … …  
    9976}
    10077
    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 
    11178} // end namespace sensor
    11279} // end namespace flair

  • trunk/lib/FlairSensorActuator/src/SimuBldc.h

    r214 r224  
    4040  * \brief Constructor
    4141  *
    42   * Construct a SimuBldc. Control part.
     42  * Construct a SimuBldc
    4343  *
    4444  * \param parent parent
    … …  
    5353
    5454  /*!
    55   * \brief Constructor
    56   *
    57   * Construct a SimuBldc. Simulation part.
    58   *
    59   * \param parent parent
    60   * \param name name
    61   * \param motors_count number of motors
    62   * \param modelId Model id
    63   * \param deviceId Bldc id of the Model
    64   */
    65   SimuBldc(const core::Object *parent, std::string name, uint8_t motors_count,
    66            uint32_t modelId,uint32_t deviceId);
    67 
    68   /*!
    6955  * \brief Destructor
    7056  *
    7157  */
    7258  ~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 speeds
    80   * \param time time when shared memory was written
    81   */
    82   void GetSpeeds(float *value,core::Time* time) const;
    8359
    8460  /*!

  • trunk/lib/FlairSensorActuator/src/SimuCamera.cpp

    r206 r224  
    3030namespace sensor {
    3131
    32 //control part
    3332SimuCamera::SimuCamera(string name,
    3433                       uint16_t width, uint16_t height, uint8_t channels,
    … …  
    4948}
    5049
    51 //simulation part
    52 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 }
    6450
    6551SimuCamera::~SimuCamera() {

  • trunk/lib/FlairSensorActuator/src/SimuCamera.h

    r158 r224  
    3535  * \brief Constructor
    3636  *
    37   * Construct a SimuCamera. Control part.
     37  * Construct a SimuCamera.
    3838        * It will be child of the FrameworkManager.
    3939  *
    … …  
    5050             uint8_t priority);
    5151
    52   /*!
    53   * \brief Constructor
    54   *
    55   * Construct a SimuCamera. Simulation part.\n
    56   * The Thread of this class should not be run.
    57   *
    58   * \param parent parent
    59   * \param name name
    60   * \param width width
    61   * \param height height
    62   * \param channels number of channels
    63   * \param modelId Model id
    64   * \param deviceId Camera id of the Model
    65   */
    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);
    6852
    6953  /*!

  • trunk/lib/FlairSensorActuator/src/SimuGps.cpp

    r214 r224  
    1818#include "SimuGps.h"
    1919#include <FrameworkManager.h>
    20 #include <GeoCoordinate.h>
    2120#include <GpsData.h>
    2221#include <SharedMem.h>
    … …  
    5756
    5857
    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 
    6958SimuGps::~SimuGps() {
    7059  SafeStop();
    … …  
    7665  dev_name << "simu" <<  modelId << "_gps_" << deviceId;
    7766  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 enu
    87     //TODO: put simulator in enu?
    88     state.e = input->ValueNoMutex(5, 0);//y simulator
    89     state.n = input->ValueNoMutex(4, 0);//x simulator
    90     state.u = -input->ValueNoMutex(6, 0);//z simulator
    91     state.ve = input->ValueNoMutex(11, 0);//vy simulator
    92     state.vn = input->ValueNoMutex(10, 0);//vx simulator
    93     input->ReleaseMutex();
    94 
    95     shmem->Write((char *)&state, sizeof(gps_states_t));
    96   }
    9767}
    9868

  • trunk/lib/FlairSensorActuator/src/SimuGps.h

    r206 r224  
    3838  * \brief Constructor
    3939  *
    40   * Construct a simulation GPS. Control part.
     40  * Construct a simulation GPS
    4141  * It will be child of the FrameworkManager.
    4242        *
    … …  
    4949  SimuGps(std::string name,
    5050          NmeaGps::NMEAFlags_t NMEAFlags, uint32_t modelId,uint32_t deviceId,uint8_t priority);
    51 
    52   /*!
    53   * \brief Constructor
    54   *
    55   * Construct a simulation GPS. Simulation part.\n
    56   * The Thread of this class should not be run.
    57   *
    58   * \param parent parent
    59   * \param name name
    60   * \param modelId Model id
    61   * \param deviceId Gps id of the Model
    62   */
    63   SimuGps(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);
    6451
    6552  /*!
    … …  
    7764  * \param data data from the parent to process
    7865  */
    79   void UpdateFrom(const core::io_data *data);
     66  void UpdateFrom(const core::io_data *data){};
    8067
    8168  /*!

  • 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

  • trunk/lib/FlairSensorActuator/src/SimuImu.h

    r186 r224  
    3838  * \brief Constructor
    3939  *
    40   * Construct a SimuImu. Control part.
     40  * Construct a SimuImu.
    4141        * It will be child of the FrameworkManager.
    4242  *
    … …  
    4848  SimuImu(std::string name,
    4949          uint32_t modelId,uint32_t deviceId, uint8_t priority);
    50 
    51   /*!
    52   * \brief Constructor
    53   *
    54   * Construct a SimuImu. Simulation part.\n
    55   * The Thread of this class should not be run.
    56   *
    57   * \param parent parent
    58   * \param name name
    59   * \param modelId Model id
    60   * \param deviceId Imu id of the Model
    61   */
    62   SimuImu(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);
    6350
    6451  /*!
    … …  
    8471  * \param data data from the parent to process
    8572  */
    86   void UpdateFrom(const core::io_data *data);
     73  void UpdateFrom(const core::io_data *data){};
    8774
    8875  typedef struct {

  • trunk/lib/FlairSensorActuator/src/SimuLaser.cpp

    r214 r224  
    1717#include "SimuLaser.h"
    1818#include <FrameworkManager.h>
    19 #include <ImuData.h>
    2019#include <SpinBox.h>
    2120#include <GroupBox.h>
    … …  
    3736  data_rate =
    3837      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;
    4838
    4939  shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId),

  • trunk/lib/FlairSensorActuator/src/SimuLaser.h

    r158 r224  
    3737  * \brief Constructor
    3838  *
    39   * Construct a SimuUs. Control part.
    40         * It will be child of the FrameworkManager.
     39  * Construct a SimuUs.
    4140  *
    4241  * \param name name
    … …  
    4746  SimuLaser(std::string name,
    4847            uint32_t modelId,uint32_t deviceId, uint8_t priority);
    49 
    50   /*!
    51   * \brief Constructor
    52   *
    53   * Construct a SimuUs. Simulation part.\n
    54   * The Thread of this class should not be run.
    55   *
    56   * \param parent parent
    57   * \param name name
    58   * \param modelId Model id
    59   * \param deviceId LaserRangeFinder id of the Model
    60   */
    61   SimuLaser(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);
    6248
    6349  /*!

  • trunk/lib/FlairSensorActuator/src/SimuUs.cpp

    r221 r224  
    1717#include "SimuUs.h"
    1818#include <FrameworkManager.h>
    19 #include <ImuData.h>
    2019#include <SpinBox.h>
    2120#include <GroupBox.h>
    … …  
    4140  shmem = new SharedMem((Thread *)this, ShMemName(modelId, deviceId), sizeof(float));
    4241
    43   SetIsReady(true);
    44 }
    45 
    46 //simulation part
    47 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  
    5342  SetIsReady(true);
    5443}

  • trunk/lib/FlairSensorActuator/src/SimuUs.h

    r158 r224  
    3737  * \brief Constructor
    3838  *
    39   * Construct a SimuUs. Control part.
     39  * Construct a SimuUs.
    4040        * It will be child of the FrameworkManager.
    4141  *
    … …  
    4747  SimuUs(std::string name,
    4848         uint32_t modelId,uint32_t deviceId, uint8_t priority);
    49 
    50   /*!
    51   * \brief Constructor
    52   *
    53   * Construct a SimuUs. Simulation part.\n
    54   * The Thread of this class should not be run.
    55   *
    56   * \param parent parent
    57   * \param name name
    58   * \param modelId Model id
    59   * \param deviceId UsRangeFinder id of the Model
    60   */
    61   SimuUs(const core::IODevice *parent, std::string name, uint32_t modelId,uint32_t deviceId);
    6249
    6350  /*!
Note: See TracChangeset for help on using the changeset viewer.