Changeset 186 in flair-src for trunk/lib


Ignore:
Timestamp:
06/23/17 17:33:03 (7 years ago)
Author:
Sanahuja Guillaume
Message:

maj imu

Location:
trunk/lib
Files:
20 edited

Legend:

Unmodified
Added
Removed

  • trunk/lib/FlairCore/src/FrameworkManager.cpp

    r123 r186  
    8181  for (vector<const Object *>::iterator it = Childs()->begin();
    8282       it < Childs()->end(); it++) {
    83     // Printf("child %i %s
    84     // %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
     83     //Printf("child %i %s %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
    8584    if ((*it)->ObjectType() == "Thread") {
    8685      if (*it != pimpl_) {
    … …  
    102101  for (vector<const Object *>::iterator it = Childs()->begin();
    103102       it < Childs()->end(); it++) {
    104     // Printf("child %i %s
    105     // %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
     103     //Printf("child %i %s %s\n",Childs()->size(),(*it)->ObjectName().c_str(),(*it)->ObjectType().c_str());
    106104    if ((*it)->ObjectType() == "IODevice") {
    107105      // Printf("del\n");
    … …  
    122120  // permet de garder l'uicom pour notifer des destructions
    123121  while (Childs()->size() != 0) {
    124     // Printf("child %i %s
    125     // %s\n",Childs()->size(),Childs()->back()->ObjectName().c_str(),Childs()->back()->ObjectType().c_str());
     122     //Printf("child %i %s %s\n",Childs()->size(),Childs()->back()->ObjectName().c_str(),Childs()->back()->ObjectType().c_str());
    126123    if (Childs()->back() != NULL)
    127124      delete Childs()->back();
    … …  
    135132void FrameworkManager::SetupConnection(string address, uint16_t port,Time watchdogTimeout,
    136133                                       size_t rcv_buf_size) {
    137   pimpl_->SetupConnection(address, port, rcv_buf_size);
     134  pimpl_->SetupConnection(address, port, watchdogTimeout,rcv_buf_size);
    138135}
    139136

  • trunk/lib/FlairCore/src/FrameworkManager_impl.cpp

    r137 r186  
    239239  gcs_watchdog = new Watchdog(
    240240      this, std::bind(&FrameworkManager_impl::ConnectionLost, this),
    241       (Time)1000000000);
     241      watchdogTimeout);
    242242  gcs_watchdog->Start();
    243243}

  • trunk/lib/FlairCore/src/OneAxisRotation_impl.cpp

    r167 r186  
    2222#include <Euler.h>
    2323#include <Quaternion.h>
     24#include <math.h>
    2425
    2526using std::string;
    … …  
    4142OneAxisRotation_impl::~OneAxisRotation_impl() {}
    4243
    43 // compute rotation of each axis through ComputeRotation(Vector3D& vector)
    4444void OneAxisRotation_impl::ComputeRotation(Quaternion &quat) const {
    45   Vector3Df rot = Vector3Df(quat.q1, quat.q2, quat.q3);
    46   ComputeRotation(rot);
    47   quat.q1 = rot.x;
    48   quat.q2 = rot.y;
    49   quat.q3 = rot.z;
     45  Quaternion rot;
     46  switch (rot_axe->CurrentIndex()) {
     47    case 0:
     48      rot=Quaternion(cosf(Euler::ToRadian(rot_value->Value()/2)),sinf(Euler::ToRadian(rot_value->Value()/2)), 0,0);
     49      break;
     50    case 1:
     51      rot=Quaternion(cosf(Euler::ToRadian(rot_value->Value()/2)),0,sinf(Euler::ToRadian(rot_value->Value()/2)),0);
     52      break;
     53    case 2:
     54      rot=Quaternion(cosf(Euler::ToRadian(rot_value->Value()/2)),0,0,sinf(Euler::ToRadian(rot_value->Value()/2)));
     55      break;
     56  }
     57  quat=quat*rot;
    5058}
    5159

  • trunk/lib/FlairCore/src/Thread_impl.cpp

    r133 r186  
    5757  this->priority = priority;
    5858  period = 100 * 1000 * 1000; // 100ms par defaut
    59   min_jitter = 1000 * 1000 * 1000;
    60   max_jitter = 0;
    61   mean_jitter = 0;
     59  min_latency = 1000 * 1000 * 1000;
     60  max_latency = 0;
     61  mean_latency = 0;
    6262  last = 0;
    6363  cpt = 0;
    … …  
    234234  next_time += period;
    235235#endif
    236   ComputeJitter(GetTime());
     236  ComputeLatency(GetTime());
    237237}
    238238
    … …  
    324324  {
    325325#ifndef __XENO__
    326 // if(last!=0)
     326  if(last!=0)
    327327#endif
    328328    { Printf("Thread::%s :\n", self->ObjectName().c_str()); }
    … …  
    346346#endif
    347347    if (last != 0) {
    348       Printf("    min jitter (ns): %lld\n", min_jitter);
    349       Printf("    max jitter (ns): %lld\n", max_jitter);
    350       Printf("    jitter moy (ns): %lld\n", mean_jitter / cpt);
     348      Printf("    min latency (ns): %lld\n", min_latency);
     349      Printf("    max latency (ns): %lld\n", max_latency);
     350      Printf("    latency moy (ns): %lld\n", mean_latency / cpt);
    351351      Printf("    itertions: %lld\n", cpt);
    352352    }
    … …  
    370370}
    371371
    372 void Thread_impl::ComputeJitter(Time time) {
     372void Thread_impl::ComputeLatency(Time time) {
    373373  Time diff, delta;
    374374  diff = time - last;
    … …  
    384384    return;
    385385
    386   if (delta > max_jitter)
    387     max_jitter = delta;
    388   if (delta < min_jitter)
    389     min_jitter = delta;
    390   mean_jitter += delta;
     386  if (delta > max_latency)
     387    max_latency = delta;
     388  if (delta < min_latency)
     389    min_latency = delta;
     390  mean_latency += delta;
    391391  cpt++;
    392392
    393   // Printf("Thread::%s jitter moy (ns):
    394   // %lld\n",self->ObjectName().c_str(),mean_jitter/cpt);
     393  // Printf("Thread::%s latency moy (ns):
     394  // %lld\n",self->ObjectName().c_str(),mean_latency/cpt);
    395395}
    396396

  • trunk/lib/FlairCore/src/unexported/FrameworkManager_impl.h

    r122 r186  
    4444  ~FrameworkManager_impl();
    4545  void SetupConnection(std::string address, uint16_t port,flair::core::Time watchdogTimeout,
    46                        size_t rcv_buf_size = 10000);
     46                       size_t rcv_buf_size);
    4747  void SetupUserInterface(std::string xml_file);
    4848  void SetupLogger(std::string log_path);

  • trunk/lib/FlairCore/src/unexported/Thread_impl.h

    r139 r186  
    5555  flair::core::ConditionVariable *cond;
    5656  uint8_t priority;
    57   flair::core::Time max_jitter, min_jitter, mean_jitter;
     57  flair::core::Time max_latency, min_latency, mean_latency;
    5858  flair::core::Time last;
    5959  uint64_t cpt;
    … …  
    6262  bool is_suspended;
    6363  void PrintStats(void);
    64   void ComputeJitter(flair::core::Time time);
     64  void ComputeLatency(flair::core::Time time);
    6565#ifdef __XENO__
    6666  RT_TASK task_rt;

  • trunk/lib/FlairFilter/src/Gx3_25_ahrs.cpp

    r173 r186  
    3030                         SerialPort *serialport, Gx3_25_imu::Command_t command,
    3131                         uint8_t priority)
    32     : Ahrs(new Gx3_25_imu( name, serialport, command, priority), name) {
     32    : Ahrs(new Gx3_25_imu( "imu", serialport, command, priority), name) {
    3333
    3434  SetIsReady(true);

  • trunk/lib/FlairFilter/src/SimuAhrs.cpp

    r167 r186  
    2929SimuAhrs::SimuAhrs(string name, uint32_t modelId,uint32_t deviceId,
    3030                   uint8_t priority)
    31     : Ahrs(new SimuImu(name, modelId,deviceId, priority), name) {
     31    : Ahrs(new SimuImu("imu", modelId,deviceId, priority), name) {
    3232  SetIsReady(true);
    3333}

  • trunk/lib/FlairMeta/src/HdsX8.cpp

    r185 r186  
    4949                       "motors", GetUavMultiplex()->MotorsCount(), i2cport));
    5050  SetUsRangeFinder(new Srf08("SRF08", i2cport, 0x70, 60));
    51   SetAhrs(new Gx3_25_ahrs("imu", imu_port,
     51  SetAhrs(new Gx3_25_ahrs("ahrs", imu_port,
    5252                          Gx3_25_imu::AccelerationAngularRateAndOrientationMatrix, 70));
    5353  SetBatteryMonitor(((BlCtrlV2 *)GetBldc())->GetBatteryMonitor());
    … …  
    5757  if(useGps=="true") {
    5858    RTDM_SerialPort *gps_port = new RTDM_SerialPort(getFrameworkManager(), "gps_port", "rtser2");
    59     SetGps(new Mb800("gps",gps_port,(NmeaGps::NMEAFlags_t)(NmeaGps::GGA|NmeaGps::VTG),40));
     59    SetGps(new Mb800("gps",gps_port,(NmeaGps::NMEAFlags_t)(NmeaGps::GGA|NmeaGps::VTG|NmeaGps::GSA),40));
    6060  }
    6161}
    … …  
    6767  ((Srf08 *)GetUsRangeFinder())->Start();
    6868  ((Ps3Eye *)GetVerticalCamera())->Start();
    69   ((Mb800 *)GetGps())->Start();
     69  if(GetGps()) ((Mb800 *)GetGps())->Start();
    7070}
    7171

  • trunk/lib/FlairMeta/src/SimuX4.cpp

    r185 r186  
    4949                       "motors", GetUavMultiplex()->MotorsCount(), simu_id,0));
    5050  SetUsRangeFinder(new SimuUs("us", simu_id,0, 60));
    51   SetAhrs(new SimuAhrs("imu", simu_id, 0,70));
     51  SetAhrs(new SimuAhrs("ahrs", simu_id, 0,70));
    5252  Tab *bat_tab = new Tab(getFrameworkManager()->GetTabWidget(), "battery");
    5353  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
    … …  
    7777  ((SimuCamera *)GetVerticalCamera())->Start();
    7878  ((SimuCamera *)GetHorizontalCamera())->Start();
    79   ((SimuGps *)GetGps())->Start();
     79  if(GetGps()) ((SimuGps *)GetGps())->Start();
    8080}
    8181

  • trunk/lib/FlairMeta/src/SimuX8.cpp

    r185 r186  
    4949                       "motors", GetUavMultiplex()->MotorsCount(), simu_id,0));
    5050  SetUsRangeFinder(new SimuUs("us", simu_id,0, 60));
    51   SetAhrs(new SimuAhrs("imu", simu_id, 0,70));
     51  SetAhrs(new SimuAhrs("ahrs", simu_id, 0,70));
    5252  Tab *bat_tab = new Tab(getFrameworkManager()->GetTabWidget(), "battery");
    5353  SetBatteryMonitor(new BatteryMonitor(bat_tab->NewRow(), "battery"));
    … …  
    7777  ((SimuCamera *)GetVerticalCamera())->Start();
    7878  ((SimuCamera *)GetHorizontalCamera())->Start();
    79   ((SimuGps *)GetGps())->Start();
     79  if(GetGps()) ((SimuGps *)GetGps())->Start();
    8080}
    8181

  • trunk/lib/FlairMeta/src/Uav.cpp

    r185 r186  
    7373  ahrs = (Ahrs *)inAhrs;
    7474  imu = (Imu *)ahrs->GetImu();
     75  //only add imu; as ahrs is son of imu, it will be logged together
    7576  getFrameworkManager()->AddDeviceToLog(imu);
    7677}

  • trunk/lib/FlairMeta/src/UavStateMachine.cpp

    r177 r186  
    460460  flagZTrajectoryFinished = false;
    461461
    462   if((altitudeState==AltitudeState_t::Stopped) && safeToFly && uav->isReadyToFly()  && uav->GetBatteryMonitor()->IsBatteryLow()==false) {
     462  if((altitudeState==AltitudeState_t::Stopped) && safeToFly && uav->isReadyToFly()  && uav->GetBatteryMonitor()->IsBatteryLow()==false && !flagConnectionLost) {
    463463    //The uav always takes off in fail safe mode
    464464    flagBatteryLow=false;
    … …  
    573573  if (getFrameworkManager()->ConnectionLost() && !flagConnectionLost) {
    574574    flagConnectionLost = true;
    575     Thread::Err("Connection lost\n");
     575   
    576576    EnterFailSafeMode();
    577577    if (altitudeState == AltitudeState_t::Stopped) {
    578       SafeStop();
     578      Thread::Warn("Connection lost\n");
     579      Thread::Warn("UAV won't take off\n");
    579580    } else {
     581      Thread::Err("Connection lost\n");
    580582      Land();
    581583    }

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

    r173 r186  
    8080  Time imuTime;
    8181  ImuData *imuData;
     82  Quaternion prevQuat;prevQuat.q0=-2;//for init
    8283  self->GetDatas(&imuData);
    8384
    … …  
    115116  }
    116117
    117   // periode a passer en argument (reglable)
    118   // ou plutot laisser la periode geree par le centrale (polling)
    119   // self->SetPeriodMS(2);
    120118  SetContinuousMode(command);
    121119
    … …  
    169167
    170168      Quaternion quaternion=matrix.ToQuaternion();
     169     
     170      //try to keep quaternion continuous (select between quaternion or -quaternion)
     171      if(prevQuat.q0!=-2) {//skip first iteration
     172        //search the greatest absolute value to avoid problems near 0 (quaternion's norm=1)
     173        if(fabs(prevQuat.q0)>=fabs(prevQuat.q1) && fabs(prevQuat.q0)>=fabs(prevQuat.q2) && fabs(prevQuat.q0)>=fabs(prevQuat.q3)) {
     174          if(prevQuat.q0*quaternion.q0<0) {//if signs are different
     175            quaternion=-quaternion;
     176          }
     177        } else if(fabs(prevQuat.q1)>=fabs(prevQuat.q0) && fabs(prevQuat.q1)>=fabs(prevQuat.q2) && fabs(prevQuat.q1)>=fabs(prevQuat.q3)) {
     178          if(prevQuat.q1*quaternion.q1<0) {//if signs are different
     179            quaternion=-quaternion;
     180          }
     181        } else if(fabs(prevQuat.q2)>=fabs(prevQuat.q0) && fabs(prevQuat.q2)>=fabs(prevQuat.q1) && fabs(prevQuat.q2)>=fabs(prevQuat.q3)) {
     182          if(prevQuat.q2*quaternion.q2<0) {//if signs are different
     183            quaternion=-quaternion;
     184          }
     185        } else if(fabs(prevQuat.q3)>=fabs(prevQuat.q0) && fabs(prevQuat.q3)>=fabs(prevQuat.q1) && fabs(prevQuat.q3)>=fabs(prevQuat.q2)) {
     186          if(prevQuat.q3*quaternion.q3<0) {//if signs are different
     187            quaternion=-quaternion;
     188          }
     189        }
     190      }
     191      prevQuat=quaternion;
     192     
    171193      self->ApplyRotation(filteredAngRates);
    172194      self->ApplyRotation(quaternion);

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

    r179 r186  
    4242  setupGroupbox = new GroupBox(sensorTab->NewRow(), name);
    4343  rotation = new OneAxisRotation(sensorTab->NewRow(), "post rotation");
     44  AddDataToLog(imuData);
    4445}
    4546

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

    r180 r186  
    108108
    109109  while (!ToBeStopped()) {
    110     SleepMS(10);
     110    //SleepMS(10);??
    111111    size = 0;
    112112    while (!ToBeStopped()) {

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

    r173 r186  
    7979    state.wy = input->ValueNoMutex(8, 0);
    8080    state.wz = input->ValueNoMutex(9, 0);
     81    state.ax = input->ValueNoMutex(13, 0);
     82    state.ay = input->ValueNoMutex(14, 0);
     83    state.az = input->ValueNoMutex(15, 0);
     84    state.mx = input->ValueNoMutex(16, 0);
     85    state.my = input->ValueNoMutex(17, 0);
     86    state.mz = input->ValueNoMutex(18, 0);
    8187    input->ReleaseMutex();
    8288
    … …  
    108114    Quaternion  quaternion(state.q0, state.q1, state.q2, state.q3);
    109115    Vector3Df angRate(state.wx, state.wy, state.wz);
     116    Vector3Df rawAcc(state.ax, state.ay, state.az);
     117    Vector3Df rawMag(state.mx, state.my, state.mz);
     118    Vector3Df rawGyr(state.wx, state.wy, state.wz);
    110119    ApplyRotation(angRate);
    111120    ApplyRotation(quaternion);
     121    ApplyRotation(rawAcc);
     122    ApplyRotation(rawMag);
     123    ApplyRotation(rawGyr);
    112124    ahrsData->SetQuaternionAndAngularRates(quaternion,angRate);
    113 
     125    imuData->SetRawAccMagAndGyr(rawAcc,rawMag,rawGyr);
    114126    imuData->SetDataTime(GetTime());
    115127    ahrsData->SetDataTime(GetTime());

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

    r158 r186  
    9494    float wy;
    9595    float wz;
     96    float ax;
     97    float ay;
     98    float az;
     99    float mx;
     100    float my;
     101    float mz;
    96102  } imu_states_t;
    97103 

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

    r137 r186  
    118118  Message *message;
    119119
    120   if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
     120  //if (getFrameworkManager()->ErrorOccured() || !ControllerInitialization()) {
     121  if (!ControllerInitialization()) {
    121122    SafeStop();
    122123    Thread::Err("An error occured, we don't proceed with the loop.\n");

  • trunk/lib/FlairSimulator/src/Model_impl.cpp

    r167 r186  
    114114  self->state[-2] = self->state[0];
    115115
    116   cvmatrix_descriptor *desc = new cvmatrix_descriptor(13, 1);
     116  cvmatrix_descriptor *desc = new cvmatrix_descriptor(19, 1);
    117117  desc->SetElementName(0, 0, "q0");
    118118  desc->SetElementName(1, 0, "q1");
    … …  
    128128  desc->SetElementName(11, 0, "vy");
    129129  desc->SetElementName(12, 0, "vz");
     130  desc->SetElementName(13, 0, "ax");
     131  desc->SetElementName(14, 0, "ay");
     132  desc->SetElementName(15, 0, "az");
     133  desc->SetElementName(16, 0, "mx");
     134  desc->SetElementName(17, 0, "my");
     135  desc->SetElementName(18, 0, "mz");
    130136  output = new cvmatrix(this, desc, floatType, "state");
    131137  delete desc;
    … …  
    318324    output->SetValueNoMutex(11, 0, self->state[0].Vel.y);
    319325    output->SetValueNoMutex(12, 0, self->state[0].Vel.z);
     326    //todo: put acc and mag
     327    output->SetValueNoMutex(13, 0, 0);
     328    output->SetValueNoMutex(14, 0, 0);
     329    output->SetValueNoMutex(15, 0, 0);
     330    output->SetValueNoMutex(16, 0, 0);
     331    output->SetValueNoMutex(17, 0, 0);
     332    output->SetValueNoMutex(18, 0, 0);
    320333    output->ReleaseMutex();
    321334    output->SetDataTime(GetTime());
Note: See TracChangeset for help on using the changeset viewer.