Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (5 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

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

    r3 r15  
    3434using namespace flair::actuator;
    3535
    36 AfroBldc_impl::AfroBldc_impl(AfroBldc* self,Layout *layout,I2cPort* i2cport) {
    37     this->self=self;
    38     this->i2cport=i2cport;
    39     nb_mot=self->MotorsCount();
     36AfroBldc_impl::AfroBldc_impl(AfroBldc *self, Layout *layout, I2cPort *i2cport) {
     37  this->self = self;
     38  this->i2cport = i2cport;
     39  nb_mot = self->MotorsCount();
    4040}
    4141
    42 AfroBldc_impl::~AfroBldc_impl() {
     42AfroBldc_impl::~AfroBldc_impl() {}
    4343
     44void AfroBldc_impl::SetMotors(float *value) {
     45  uint16_t tosend_value[nb_mot];
     46
     47  for (int i = 0; i < nb_mot; i++)
     48    tosend_value[i] = (uint16_t)(MAX_VALUE * value[i]);
     49
     50  i2cport->GetMutex();
     51
     52  for (int i = 0; i < nb_mot; i++) {
     53    i2cport->SetSlave(BASE_ADDRESS + i);
     54    WriteValue(tosend_value[i]);
     55  }
     56  i2cport->ReleaseMutex();
    4457}
    4558
    46 void AfroBldc_impl::SetMotors(float* value) {
    47     uint16_t tosend_value[nb_mot];
     59// I2cPort mutex must be taken before calling this function
     60void AfroBldc_impl::WriteValue(uint16_t value) {
     61  uint8_t tx[2];
     62  ssize_t written;
    4863
    49     for(int i=0; i<nb_mot; i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
    50 
    51     i2cport->GetMutex();
    52 
    53     for(int i=0; i<nb_mot; i++) {
    54         i2cport->SetSlave(BASE_ADDRESS+i);
    55         WriteValue(tosend_value[i]);
    56     }
    57     i2cport->ReleaseMutex();
     64  tx[0] = (uint8_t)(value >> 3); // msb
     65  tx[1] = (value & 0x07);        // lsb
     66  written = i2cport->Write(tx, 2);
     67  if (written < 0) {
     68    self->Err("rt_dev_write error (%s)\n", strerror(-written));
     69  } else if (written != sizeof(tx)) {
     70    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
     71  }
    5872}
    59 
    60 //I2cPort mutex must be taken before calling this function
    61 void AfroBldc_impl::WriteValue(uint16_t value) {
    62     uint8_t tx[2];
    63     ssize_t written;
    64 
    65     tx[0]=(uint8_t)(value>>3);//msb
    66     tx[1]=(value&0x07);//lsb
    67     written =i2cport->Write(tx, 2);
    68     if(written<0) {
    69         self->Err("rt_dev_write error (%s)\n",strerror(-written));
    70     } else if (written != sizeof(tx)) {
    71         self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
    72     }
    73 }
Note: See TracChangeset for help on using the changeset viewer.