Revision b4885314 components/gyro/l3g4200d.cpp
| components/gyro/l3g4200d.cpp | ||
|---|---|---|
| 1 |
#include <ch.hpp> |
|
| 2 |
#include <hal.h> |
|
| 3 |
#include <chprintf.h> |
|
| 1 | 4 |
#include <string.h> |
| 2 | 5 |
|
| 3 | 6 |
#include <amiro/util/util.h> |
| 4 | 7 |
#include <amiro/bus/spi/HWSPIDriver.hpp> |
| 5 | 8 |
#include <amiro/gyro/l3g4200d.hpp> |
| 9 |
#include <amiro/Constants.h> |
|
| 10 |
|
|
| 11 |
using namespace chibios_rt; |
|
| 6 | 12 |
|
| 7 | 13 |
namespace amiro {
|
| 8 | 14 |
|
| 9 | 15 |
L3G4200D::L3G4200D(HWSPIDriver *driver) |
| 10 |
: driver(driver) {
|
|
| 11 |
|
|
| 16 |
: driver(driver), |
|
| 17 |
udpsPerTic(175000), |
|
| 18 |
period_us(100000) {
|
|
| 19 |
this->period_ms = this->period_us * 1e-3; |
|
| 20 |
this->period_st = US2ST(this->period_us); |
|
| 12 | 21 |
} |
| 13 | 22 |
|
| 14 | 23 |
L3G4200D::~L3G4200D() {
|
| ... | ... | |
| 21 | 30 |
} |
| 22 | 31 |
|
| 23 | 32 |
msg_t L3G4200D::main() {
|
| 24 |
|
|
| 25 |
this->setName("L3g4200d");
|
|
| 33 |
systime_t time = System::getTime(); |
|
| 34 |
this->setName("l3g4200d");
|
|
| 26 | 35 |
|
| 27 | 36 |
while (!this->shouldTerminate()) {
|
| 37 |
time += this->period_st; |
|
| 28 | 38 |
|
| 29 | 39 |
updateSensorData(); |
| 40 |
calcAngular(); |
|
| 30 | 41 |
|
| 31 | 42 |
this->eventSource.broadcastFlags(1); |
| 32 | 43 |
|
| 33 |
this->waitAnyEventTimeout(ALL_EVENTS, MS2ST(200)); |
|
| 34 |
|
|
| 44 |
if (time >= System::getTime()) {
|
|
| 45 |
chThdSleepUntil(time); |
|
| 46 |
} else {
|
|
| 47 |
chprintf((BaseSequentialStream*) &SD1, "WARNING l3g4200d: Unable to keep track\r\n"); |
|
| 48 |
} |
|
| 35 | 49 |
} |
| 36 | 50 |
return RDY_OK; |
| 37 | 51 |
} |
| ... | ... | |
| 39 | 53 |
int16_t |
| 40 | 54 |
L3G4200D:: |
| 41 | 55 |
getAngularRate(const uint8_t axis) {
|
| 42 |
|
|
| 43 | 56 |
return this->angularRate[axis]; |
| 57 |
} |
|
| 44 | 58 |
|
| 59 |
int32_t |
|
| 60 |
L3G4200D:: |
|
| 61 |
getAngularRate_udps(const uint8_t axis) {
|
|
| 62 |
return uint32_t(this->angularRate[axis]) * this->udpsPerTic; |
|
| 63 |
} |
|
| 64 |
|
|
| 65 |
int32_t |
|
| 66 |
L3G4200D:: |
|
| 67 |
getAngular(const uint8_t axis) {
|
|
| 68 |
return this->angular[axis]; |
|
| 69 |
} |
|
| 70 |
|
|
| 71 |
int32_t |
|
| 72 |
L3G4200D:: |
|
| 73 |
getAngular_ud(const uint8_t axis) {
|
|
| 74 |
const int32_t angularRate_mdps = this->getAngularRate_udps(axis) * 1e-3; |
|
| 75 |
return angularRate_mdps * (this->integrationTic * this->period_ms); |
|
| 45 | 76 |
} |
| 46 | 77 |
|
| 78 |
void |
|
| 79 |
L3G4200D:: |
|
| 80 |
calcAngular() {
|
|
| 81 |
// Need to check for overflow! |
|
| 82 |
++this->integrationTic; |
|
| 83 |
chSysLock(); |
|
| 84 |
this->angular[L3G4200D::AXIS_X] += int32_t(this->angularRate[L3G4200D::AXIS_X]); |
|
| 85 |
this->angular[L3G4200D::AXIS_Y] += int32_t(this->angularRate[L3G4200D::AXIS_Y]); |
|
| 86 |
this->angular[L3G4200D::AXIS_Z] += int32_t(this->angularRate[L3G4200D::AXIS_Z]); |
|
| 87 |
chSysUnlock(); |
|
| 88 |
} |
|
| 89 |
|
|
| 90 |
// TODO: Outsource, so that everyone who needs this has an own instance of the integrator |
|
| 91 |
void |
|
| 92 |
L3G4200D:: |
|
| 93 |
angularReset() {
|
|
| 94 |
this->angular[L3G4200D::AXIS_X] = 0; |
|
| 95 |
this->angular[L3G4200D::AXIS_Y] = 0; |
|
| 96 |
this->angular[L3G4200D::AXIS_Z] = 0; |
|
| 97 |
this->integrationTic = 0; |
|
| 98 |
} |
|
| 99 |
|
|
| 100 |
|
|
| 47 | 101 |
void L3G4200D::updateSensorData() {
|
| 48 | 102 |
|
| 49 | 103 |
const size_t buffer_size = offsetof(L3G4200D::registers, OUT_Z) |
| ... | ... | |
| 62 | 116 |
// assemble data |
| 63 | 117 |
sreg = buffer[1]; |
| 64 | 118 |
|
| 119 |
chSysLock(); |
|
| 65 | 120 |
if (sreg & L3G4200D::XDA) |
| 66 | 121 |
this->angularRate[L3G4200D::AXIS_X] = (buffer[3] << 8) + buffer[2]; |
| 67 | 122 |
|
| ... | ... | |
| 70 | 125 |
|
| 71 | 126 |
if (sreg & L3G4200D::ZDA) |
| 72 | 127 |
this->angularRate[L3G4200D::AXIS_Z] = (buffer[7] << 8) + buffer[6]; |
| 73 |
|
|
| 128 |
chSysUnlock(); |
|
| 74 | 129 |
} |
| 75 | 130 |
|
| 76 | 131 |
msg_t L3G4200D::configure(const L3G4200DConfig *config) {
|
| ... | ... | |
| 95 | 150 |
buffer[5] = config->ctrl5; |
| 96 | 151 |
this->driver->write(buffer, 6); |
| 97 | 152 |
|
| 153 |
// Handle the new update time |
|
| 154 |
switch(config->ctrl1 & L3G4200D::DR_MASK) {
|
|
| 155 |
case L3G4200D::DR_100_HZ: this->period_us = 10000; break; |
|
| 156 |
case L3G4200D::DR_200_HZ: this->period_us = 5000; break; |
|
| 157 |
case L3G4200D::DR_400_HZ: this->period_us = 2500; break; |
|
| 158 |
case L3G4200D::DR_800_HZ: this->period_us = 1250; break; |
|
| 159 |
} |
|
| 160 |
this->period_st = US2ST(this->period_st); |
|
| 161 |
|
|
| 162 |
// Handle the new full scale |
|
| 163 |
switch(config->ctrl1 & L3G4200D::FS_MASK) {
|
|
| 164 |
case L3G4200D::FS_250_DPS: this->udpsPerTic = 8750; break; |
|
| 165 |
case L3G4200D::FS_500_DPS: this->udpsPerTic = 17500; break; |
|
| 166 |
case L3G4200D::FS_2000_DPS: this->udpsPerTic = 70000; break; |
|
| 167 |
} |
|
| 168 |
|
|
| 169 |
// Reset the integration |
|
| 170 |
this->angularReset(); |
|
| 171 |
|
|
| 98 | 172 |
return RDY_OK; |
| 99 | 173 |
|
| 100 | 174 |
} |
Also available in: Unified diff