added ability to specify arbitrary roll/pitch/yaw translation for board mounting by alu.

this allows to mount FC vertically/upside down/etc easily.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@445 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61

src/cli.c

@@ -127,6 +127,9 @@ const clivalue_t valueTable[] = {
     { "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
     { "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
     { "align_mag", VAR_UINT8, &mcfg.mag_align, 0, 8 },
+    { "align_board_roll", VAR_INT16, &mcfg.board_align_roll, -180, 360 },
+    { "align_board_pitch", VAR_INT16, &mcfg.board_align_pitch, -180, 360 },
+    { "align_board_yaw", VAR_INT16, &mcfg.board_align_yaw, -180, 360 },
     { "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 },
     { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 },
     { "moron_threshold", VAR_UINT8, &mcfg.moron_threshold, 0, 128 },

src/config.c

@@ -13,7 +13,7 @@ master_t mcfg;  // master config struct with data independent from profiles
 config_t cfg;   // profile config struct
 const char rcChannelLetters[] = "AERT1234";
 
-static const uint8_t EEPROM_CONF_VERSION = 53;
+static const uint8_t EEPROM_CONF_VERSION = 54;
 static uint32_t enabledSensors = 0;
 static void resetConf(void);
 
@@ -180,6 +180,9 @@ static void resetConf(void)
     mcfg.gyro_align = ALIGN_DEFAULT;
     mcfg.acc_align = ALIGN_DEFAULT;
     mcfg.mag_align = ALIGN_DEFAULT;
+    mcfg.board_align_roll = 0;
+    mcfg.board_align_pitch = 0;
+    mcfg.board_align_yaw = 0;
     mcfg.acc_hardware = ACC_DEFAULT;     // default/autodetect
     mcfg.yaw_control_direction = 1;
     mcfg.moron_threshold = 32;

src/main.c

@@ -49,6 +49,7 @@ int main(void)
     }
 
     adcInit(&adc_params);
+    initBoardAlignment();
 
     // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
     sensorsSet(SENSORS_SET);

src/mw.h

@@ -234,6 +234,9 @@ typedef struct master_t {
     sensor_align_e gyro_align;              // gyro alignment
     sensor_align_e acc_align;               // acc alignment
     sensor_align_e mag_align;               // mag alignment
+    int16_t board_align_roll;               // board alignment correction in roll (deg)
+    int16_t board_align_pitch;              // board alignment correction in pitch (deg)
+    int16_t board_align_yaw;                // board alignment correction in yaw (deg)
     int8_t yaw_control_direction;           // change control direction of yaw (inverted, normal)
     uint8_t acc_hardware;                   // Which acc hardware to use on boards with more than one device
     uint16_t gyro_lpf;                      // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.
@@ -285,7 +288,6 @@ typedef struct core_t {
     serialPort_t *rcvrport;
     bool useServo;
     uint8_t numRCChannels;
-
 } core_t;
 
 typedef struct flags_t {

src/utils.c

@@ -1,6 +1,9 @@
 #include "board.h"
 #include "mw.h"
 
+static bool standardBoardAlignment = true;     // board orientation correction
+static float boardRotation[3][3];              // matrix
+
 int constrain(int amt, int low, int high)
 {
     if (amt < low)
@@ -11,6 +14,46 @@ int constrain(int amt, int low, int high)
         return amt;
 }
 
+void initBoardAlignment(void)
+{
+    float roll, pitch, yaw;
+
+    // standard alignment, nothing to calculate
+    if (!mcfg.board_align_roll && !mcfg.board_align_pitch && !mcfg.board_align_yaw)
+        return;
+
+    standardBoardAlignment = false;
+
+    // deg2rad
+    roll = mcfg.board_align_roll * M_PI / 180.0f;
+    pitch = mcfg.board_align_pitch * M_PI / 180.0f;
+    yaw = mcfg.board_align_yaw * M_PI / 180.0f;
+
+    // define rotation matrix
+    boardRotation[0][0] = cosf(roll) * cosf(pitch);
+    boardRotation[0][1] = cosf(roll) * sinf(pitch) * sinf(yaw) - sinf(roll) * cosf(yaw);
+    boardRotation[0][2] = cosf(roll) * sinf(pitch) * cosf(yaw) + sinf(roll) * sinf(yaw);
+
+    boardRotation[1][0] = sinf(roll) * cosf(pitch);
+    boardRotation[1][1] = sinf(roll) * sinf(pitch) * sinf(yaw) + cosf(roll) * cosf(yaw);
+    boardRotation[1][2] = sinf(roll) * sinf(pitch) * cosf(yaw) - cosf(roll) * sinf(yaw);
+
+    boardRotation[2][0] = -sinf(pitch);
+    boardRotation[2][1] = cosf(pitch) * sinf(yaw);
+    boardRotation[2][2] = cosf(pitch) * cosf(yaw);
+}
+
+void alignBoard(int16_t *vec)
+{
+    int16_t x = vec[X];
+    int16_t y = vec[Y];
+    int16_t z = vec[Z];
+
+    vec[X] = boardRotation[0][0] * x + boardRotation[0][1] * y + boardRotation[0][2] * z;
+    vec[Y] = boardRotation[1][0] * x + boardRotation[1][1] * y + boardRotation[1][2] * z;
+    vec[Z] = boardRotation[2][0] * x + boardRotation[2][1] * y + boardRotation[2][2] * z;
+}
+
 void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
 {
     switch (rotation) {
@@ -57,4 +100,7 @@ void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation)
         default:
             break;
     }
+
+    if (!standardBoardAlignment)
+        alignBoard(dest);
 }

src/utils.h

@@ -3,3 +3,4 @@
 int constrain(int amt, int low, int high);
 // sensor orientation
 void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);
+void initBoardAlignment(void);