Revision f8cf404d components/accel/lis331dlh.cpp
| components/accel/lis331dlh.cpp | ||
|---|---|---|
| 6 | 6 |
#include <chprintf.h> |
| 7 | 7 |
#include <cmath> // abs() |
| 8 | 8 |
#include <amiro/Constants.h> |
| 9 |
#include <global.hpp> |
|
| 9 | 10 |
|
| 10 | 11 |
namespace amiro {
|
| 11 | 12 |
|
| 13 |
extern Global global; |
|
| 14 |
|
|
| 12 | 15 |
LIS331DLH:: |
| 13 | 16 |
LIS331DLH(HWSPIDriver *driver) : |
| 14 | 17 |
driver(driver), |
| ... | ... | |
| 238 | 241 |
this->configure(&test_accel_run_cfg); |
| 239 | 242 |
BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs)); |
| 240 | 243 |
// Grep some values and build the mean value |
| 241 |
chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc std values\n");
|
|
| 244 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\nACC: Get acc std values\n");
|
|
| 242 | 245 |
int32_t stdAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
|
| 243 | 246 |
->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration( |
| 244 | 247 |
LIS331DLH::AXIS_Z) }; |
| ... | ... | |
| 248 | 251 |
for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
| 249 | 252 |
accel = int32_t(this->getAcceleration(i)); |
| 250 | 253 |
stdAccValues[i] = (stdAccValues[i] * n + accel) / (n + 1); |
| 251 |
chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 254 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 252 | 255 |
accel < 0 ? -accel : accel, stdAccValues[i]); |
| 253 | 256 |
} |
| 254 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 257 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 255 | 258 |
} |
| 256 | 259 |
|
| 257 | 260 |
// 2. Apply negative offset |
| ... | ... | |
| 262 | 265 |
this->configure(&test_accel_run_cfg); |
| 263 | 266 |
BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs)); |
| 264 | 267 |
// Grep some values and build the mean value |
| 265 |
chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc neg values\n");
|
|
| 268 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\nACC: Get acc neg values\n");
|
|
| 266 | 269 |
int16_t negAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
|
| 267 | 270 |
->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration( |
| 268 | 271 |
LIS331DLH::AXIS_Z) }; |
| ... | ... | |
| 272 | 275 |
for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
| 273 | 276 |
accel = int32_t(this->getAcceleration(i)); |
| 274 | 277 |
negAccValues[i] = (negAccValues[i] * n + accel) / (n + 1); |
| 275 |
chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 278 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 276 | 279 |
accel < 0 ? -accel : accel, negAccValues[i]); |
| 277 | 280 |
} |
| 278 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 281 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 279 | 282 |
} |
| 280 | 283 |
|
| 281 | 284 |
// 2. Apply positive offset |
| ... | ... | |
| 286 | 289 |
this->configure(&test_accel_run_cfg); |
| 287 | 290 |
BaseThread::sleep(MS2ST(sleepBetweenMeasurementMs)); |
| 288 | 291 |
// Grep some values and build the mean value |
| 289 |
chprintf((BaseSequentialStream*) &SD1, "\nACC: Get acc pos values\n");
|
|
| 292 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\nACC: Get acc pos values\n");
|
|
| 290 | 293 |
int16_t posAccValues[3] = { this->getAcceleration(LIS331DLH::AXIS_X), this
|
| 291 | 294 |
->getAcceleration(LIS331DLH::AXIS_Y), this->getAcceleration( |
| 292 | 295 |
LIS331DLH::AXIS_Z) }; |
| ... | ... | |
| 296 | 299 |
for (uint8_t i = LIS331DLH::AXIS_X; i <= LIS331DLH::AXIS_Z; i++) {
|
| 297 | 300 |
accel = int32_t(this->getAcceleration(i)); |
| 298 | 301 |
posAccValues[i] = (posAccValues[i] * n + accel) / (n + 1); |
| 299 |
chprintf((BaseSequentialStream*) &SD1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 302 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "%c%d:%d ", accel < 0 ? '-' : '+',
|
|
| 300 | 303 |
accel < 0 ? -accel : accel, posAccValues[i]); |
| 301 | 304 |
} |
| 302 |
chprintf((BaseSequentialStream*) &SD1, "\n");
|
|
| 305 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n");
|
|
| 303 | 306 |
} |
| 304 | 307 |
|
| 305 | 308 |
// Get the amplitude change and compare it to the standard values from LIS331DLH manual table 3 |
| ... | ... | |
| 308 | 311 |
const int32_t ampXmin = 120, ampYmin = 120, ampZmin = 140; |
| 309 | 312 |
|
| 310 | 313 |
// TEST |
| 311 |
chprintf((BaseSequentialStream*) &SD1, "\n\nACC: Testresult\n");
|
|
| 314 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "\n\nACC: Testresult\n");
|
|
| 312 | 315 |
amp = std::abs( |
| 313 | 316 |
stdAccValues[LIS331DLH::AXIS_X] - negAccValues[LIS331DLH::AXIS_X]); |
| 314 | 317 |
if (amp < ampXmin || amp > ampXmax) |
| 315 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Negative x-axis faulty\n");
|
|
| 318 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Negative x-axis faulty\n");
|
|
| 316 | 319 |
amp = std::abs( |
| 317 | 320 |
stdAccValues[LIS331DLH::AXIS_Y] - negAccValues[LIS331DLH::AXIS_Y]); |
| 318 | 321 |
if (amp < ampYmin || amp > ampYmax) |
| 319 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Negative y-axis faulty\n");
|
|
| 322 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Negative y-axis faulty\n");
|
|
| 320 | 323 |
amp = std::abs( |
| 321 | 324 |
stdAccValues[LIS331DLH::AXIS_Z] - negAccValues[LIS331DLH::AXIS_Z]); |
| 322 | 325 |
if (amp < ampZmin || amp > ampZmax) |
| 323 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Negative z-axis faulty\n");
|
|
| 326 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Negative z-axis faulty\n");
|
|
| 324 | 327 |
amp = std::abs( |
| 325 | 328 |
stdAccValues[LIS331DLH::AXIS_X] - posAccValues[LIS331DLH::AXIS_X]); |
| 326 | 329 |
if (amp < ampXmin || amp > ampXmax) |
| 327 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Positive x-axis faulty\n");
|
|
| 330 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Positive x-axis faulty\n");
|
|
| 328 | 331 |
amp = std::abs( |
| 329 | 332 |
stdAccValues[LIS331DLH::AXIS_Y] - posAccValues[LIS331DLH::AXIS_Y]); |
| 330 | 333 |
if (amp < ampYmin || amp > ampYmax) |
| 331 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Positive y-axis faulty\n");
|
|
| 334 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Positive y-axis faulty\n");
|
|
| 332 | 335 |
amp = std::abs( |
| 333 | 336 |
stdAccValues[LIS331DLH::AXIS_Z] - posAccValues[LIS331DLH::AXIS_Z]); |
| 334 | 337 |
if (amp < ampZmin || amp > ampZmax) |
| 335 |
chprintf((BaseSequentialStream*) &SD1, "ACC: Positive z-axis faulty\n");
|
|
| 338 |
chprintf((BaseSequentialStream*) &global.sercanmux1, "ACC: Positive z-axis faulty\n");
|
|
| 336 | 339 |
|
| 337 | 340 |
// Write back the original config |
| 338 | 341 |
this->configure(config); |
Also available in: Unified diff