From b02dbca9d4a2b228da4cc598baccca33f74f864f Mon Sep 17 00:00:00 2001 From: Mike McCauley Date: Tue, 24 Sep 2013 21:57:19 +1000 Subject: [PATCH] AP_InertialSensor: fix orientation of Flymaple sensors --- .../AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt | 52 ++++++++++++++++++- .../AP_InertialSensor_Flymaple.cpp | 16 +++--- 2 files changed, 61 insertions(+), 7 deletions(-) diff --git a/libraries/AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt b/libraries/AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt index 1ac110f832..96591c70a5 100644 --- a/libraries/AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt +++ b/libraries/AP_HAL_FLYMAPLE/FlymaplePortingNotes.txt @@ -96,6 +96,7 @@ cd libmaple wget http://static.leaflabs.com/pub/codesourcery/gcc-arm-none-eabi-latest-linux32.tar.gz tar xvzf gcc-arm-none-eabi-latest-linux32.tar.gz export PATH=$PATH:~/libmaple/arm/bin +make edit ArduPlane/config.mk to be something like: #config.mk START @@ -107,7 +108,8 @@ HAL_BOARD ?= HAL_BOARD_FLYMAPLE PORT = /dev/ttyACM0 # You must provide the path to the libmaple library directory: LIBMAPLE_PATH = $(HOME)/libmaple -# Also, the ARM compiler tools MUST be in your current PATH +# Also, the ARM compiler tools MUST be in your current PATH like: +# export PATH=$PATH:~/libmaple/arm/bin #config.mk END Interrupt disabling on ARM @@ -132,6 +134,54 @@ On AVR, cli()/sei() is dangerous both in ISRs *and* when nested. On ARM, noInterrupts()/interrupts() is only dangerous when nested. +Sensor Orientation + +The Flymaple board has no clear indication about which way is meant to be +'forward' or 'right', so we have adopted the following convention: + +Aircraft 'Forward' is in the direction of the arrow marked 'Pitch' on the board, ie +towards pin 0. + +Aircraft 'Right' is towards the bottom right corner of the board, towards pin 20 and +the 5V regulator + +Aircraft 'Down' is away from the copper side of the board: 'right way up' is with +component side up. + +Here in SE Queensland, in the southern hemisphere, the local mag field is +substantially vertical (down? is that correct?), and so the following simple +tests of the board should give the following results, using the mavproxy +graphing tools, and with a board orientation parameter of none: + +The aircraft coordinate system of ardupilot is: + +X +ve forward +Y +ve right +Z +ve down + +Compass +Orientation Results +Level, right way up Z -ve +Left side down Y +ve +Nose up X +ve +(ie positive when that axis is pointing away from the earth, at least where I +am) + +Accelerometer +Orientation Results +Level, right way up Z -ve +Left side down Y +ve +Nose up X +ve +(ie positive when that axis is pointing away from the earth, and consistent +with compass in southern hemisphere) + +Gyro +Rotation Results +Yawing to right Z +ve +Rolling to right X +ve +Pitching up Y +ve + +(ie right hand curl rule relative to the given axis) Remaining issues: diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp index e46d2d2080..5d48d819f4 100644 --- a/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp +++ b/libraries/AP_InertialSensor/AP_InertialSensor_Flymaple.cpp @@ -190,9 +190,13 @@ void AP_InertialSensor_Flymaple::_accumulate(void) uint8_t buffer[8]; if (hal.i2c->readRegisters(FLYMAPLE_ACCELEROMETER_ADDRESS, FLYMAPLE_ACCELEROMETER_ADXLREG_DATAX0, 8, buffer) == 0) { - int16_t x = -((((int16_t)buffer[1]) << 8) | buffer[0]); // X axis - int16_t y = (((int16_t)buffer[3]) << 8) | buffer[2]; // Y axis - int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // Z axis + // The order is a bit wierd here since the standard we have adopted for Flymaple + // sensor orientation is different to what the board designers intended + // Caution, to support alternative chip orientations on other bords, may + // need to add a chip orientation rotate + int16_t y = ((((int16_t)buffer[1]) << 8) | buffer[0]); // chip X axis + int16_t x = (((int16_t)buffer[3]) << 8) | buffer[2]; // chip Y axis + int16_t z = -((((int16_t)buffer[5]) << 8) | buffer[4]); // chip Z axis _accel_sum += Vector3f(x, y, z); _accel_sum_count++; _last_accel_timestamp = hal.scheduler->micros(); @@ -201,9 +205,9 @@ void AP_InertialSensor_Flymaple::_accumulate(void) // Read gyro if (hal.i2c->readRegisters(FLYMAPLE_GYRO_ADDRESS, FLYMAPLE_GYRO_GYROX_H, 6, buffer) == 0) { - int16_t x = (((int16_t)buffer[0]) << 8) | buffer[1]; // X axis - int16_t y = (((int16_t)buffer[2]) << 8) | buffer[3]; // Y axis - int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // Z axis + int16_t y = -(((int16_t)buffer[0]) << 8) | buffer[1]; // chip X axis + int16_t x = -(((int16_t)buffer[2]) << 8) | buffer[3]; // chip Y axis + int16_t z = -((((int16_t)buffer[4]) << 8) | buffer[5]); // chip Z axis _gyro_sum += Vector3f(x, y, z); _gyro_sum_count++; _last_gyro_timestamp = hal.scheduler->micros();