Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 04:45:24 +03:00
Code Issues Wiki Activity

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

Browse source 
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
This commit is contained in:
timecop@gmail.com 2013-10-19 09:56:29 +00:00
parent 6b93f06e49
commit 8d7f82dc75
6 changed files with 58 additions and 2 deletions
Download patch file Download diff file Expand all files Collapse all files

46
src/utils.c
View file

@ -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);
}