Revision f336542d

View differences:

components/Odometry.cpp
19 19

  
20 20

  
21 21
Odometry::Odometry(MotorIncrements* mi, L3G4200D* gyroscope)
22
    : BaseStaticThread<1024>(),
22
    : BaseStaticThread<512>(),
23 23
      motorIncrements(mi),
24 24
      gyro(gyroscope),
25 25
      eventSource(),
......
82 82

  
83 83
void Odometry::setError(float* Cp3x3) {
84 84
  chSysLock();
85
//   float** test;
86 85
    Matrix::copy<float>(Cp3x3,3,3, &(this->Cp3x3[0]),3,3);
87
//     Matrix::copy<float>(Cp3x3,3,3, test,3,3);
88 86
  chSysUnlock();
89 87
}
90 88

  
91 89
void Odometry::resetError() {
92
//   float zeroMatrix[9] = {};
93
//   this->setError(&(zeroMatrix[0]));
94 90
  Matrix::init<float>(&(this->Cp3x3[0]),3,3,0.0f);
95 91
}
96 92

  
......
105 101
  while (!this->shouldTerminate()) {
106 102
    time += MS2ST(this->period);
107 103

  
108
    // Update the base distance, because it may change after a calibration
104
    // Update the base distance, because it may have changed after a calibration
109 105
    this->updateWheelBaseDistance();
110 106

  
111 107
    // Get the actual speed
......
120 116
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "distance_left:%f distance_right:%f", this->distance[0],this->distance[1]);
121 117
//     chprintf((BaseSequentialStream*) &global.sercanmux1, "\r\n");
122 118

  
123
  if (time >= System::getTime()) {
124
      chThdSleepUntil(time);
119
    if (time >= System::getTime()) {
120
        chThdSleepUntil(time);
125 121
    } else {
126
      chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
127
  }
122
        chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING Odometry: Unable to keep track\r\n");
123
    }
128 124
  }
129 125

  
130 126
  return true;
......
134 130

  
135 131
  // Get the temporary position and error
136 132
  float Cp3x3[9];
137
  uint32_t angular_ud;
133
  int32_t angular_ud;
134
  int32_t angularRate_udps;
138 135
  chSysLock();
139 136
    float pX = this->pX;
140 137
    float pY = this->pY;
141 138
    float pPhi = this->pPhi;
142 139
    Matrix::copy<float>(this->Cp3x3,3,3,Cp3x3,3,3);
143
    // Get the differentail gyro information and reset it
144
    angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
145
  gyro->angularReset();
140
    // TODO Get the gyro (or gyro rate) information and do something with it
141
    // angular_ud = gyro->getAngular_ud(L3G4200D::AXIS_Z);
142
    // angularRate_udps = gyro->getAngularRate_udps(L3G4200D::AXIS_Z);
146 143
  chSysUnlock();
147 144

  
148 145
  ////////////////
......
150 147
  ////////////////
151 148

  
152 149
  // TMP: Rotated angular
153
  // float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
154
  float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
150
  float dPhi = (this->distance[RIGHT_WHEEL] - this->distance[LEFT_WHEEL]) / this->wheelBaseDistanceSI;
151
  // TODO Calculate the differential angel dPhi from either the angular (1. line) or angular rate (2.+3. line)
152
  // float dPhi = ((float(angular_ud * 1e-3) * M_PI ) * 1e-3) / 180.0f;
153
  // const float angular_md = float((angularRate_udps * this->period / constants::millisecondsPerSecond) * 1e-3);
154
  // float dPhi = ((angular_md * M_PI) * 1e-3) / 180.0f;
155 155

  
156 156
  // TMP: Moved distance
157 157
  float dDistance = (this->distance[RIGHT_WHEEL] + this->distance[LEFT_WHEEL]) / 2.0f;
components/gyro/l3g4200d.cpp
37 37
  this->setName("l3g4200d");
38 38

  
39 39
  while (!this->shouldTerminate()) {
40
  time += this->period_st;
40
    time += this->period_st;
41 41

  
42 42
    updateSensorData();
43 43
    calcAngular();
......
48 48
      chThdSleepUntil(time);
49 49
    } else {
50 50
      chprintf((BaseSequentialStream*) &global.sercanmux1, "WARNING l3g4200d: Unable to keep track\r\n");
51
  }
51
    }
52 52
  }
53 53
  return RDY_OK;
54 54
}
......
62 62
int32_t
63 63
L3G4200D::
64 64
getAngularRate_udps(const uint8_t axis) {
65
  return uint32_t(this->angularRate[axis]) * this->udpsPerTic;
65
  return int32_t(this->angularRate[axis]) * this->udpsPerTic;
66 66
}
67 67

  
68 68
int32_t
......
160 160
    case L3G4200D::DR_400_HZ: this->period_us =  2500; break;
161 161
    case L3G4200D::DR_800_HZ: this->period_us =  1250; break;
162 162
  }
163
  this->period_st = US2ST(this->period_st);
163
  this->period_st = US2ST(this->period_us);
164
  this->period_ms = this->period_us * 1e-3;
164 165

  
165 166
  // Handle the new full scale
166 167
  switch(config->ctrl1 & L3G4200D::FS_MASK) {
devices/DiWheelDrive/main.cpp
849 849

  
850 850
  global.increments.start();  // Start the qei driver
851 851

  
852
  // Start the three axes gyroscope
853
  global.l3g4200d.configure(&global.gyro_run_config);
854
  global.l3g4200d.start(NORMALPRIO+5);
855

  
852 856
  global.odometry.start(NORMALPRIO + 20);
853 857

  
854 858
  global.robot.start(HIGHPRIO - 1);
......
867 871
  global.lis331dlh.configure(&global.accel_run_config);
868 872
  global.lis331dlh.start(NORMALPRIO+4);
869 873

  
870
  // Start the three axes gyroscope
871
  global.l3g4200d.configure(&global.gyro_run_config);
872
  global.l3g4200d.start(NORMALPRIO+5);
873

  
874 874
  // Start the user thread
875 875
  global.userThread.start(NORMALPRIO);
876 876

  
include/amiro/Odometry.h
8 8

  
9 9
namespace amiro {
10 10

  
11
  class Odometry : public chibios_rt::BaseStaticThread<1024> {
11
  class Odometry : public chibios_rt::BaseStaticThread<512> {
12 12
  public:
13 13
    /**
14 14
     * Constructor

Also available in: Unified diff