Browse Source

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
master
timecop@gmail.com 11 years ago
parent
commit
8d7f82dc75
  1. 3
      src/cli.c
  2. 5
      src/config.c
  3. 1
      src/main.c
  4. 4
      src/mw.h
  5. 46
      src/utils.c
  6. 1
      src/utils.h

3
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 },

5
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;

1
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);

4
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 {

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

1
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);
Loading…
Cancel
Save