1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-16 21:05:35 +03:00

Decouple board alignment code.

This commit is contained in:
Dominic Clifton 2014-04-17 19:28:38 +01:00
parent 59a5846146
commit 41b5a01958
7 changed files with 40 additions and 20 deletions

View file

@ -1,25 +1,38 @@
#include "board.h" #include "stdbool.h"
#include "mw.h" #include "stdint.h"
#include "math.h"
#include "maths.h"
#include "axis.h"
#include "sensors_common.h"
#include "boardalignment.h"
static bool standardBoardAlignment = true; // board orientation correction static bool standardBoardAlignment = true; // board orientation correction
static float boardRotation[3][3]; // matrix static float boardRotation[3][3]; // matrix
void initBoardAlignment(void) static bool isBoardAlignmentStandard(boardAlignment_t *boardAlignment)
{
return !boardAlignment->rollDegrees && !boardAlignment->pitchDegrees && !boardAlignment->yawDegrees;
}
void initBoardAlignment(boardAlignment_t *boardAlignment)
{ {
float roll, pitch, yaw; float roll, pitch, yaw;
float cosx, sinx, cosy, siny, cosz, sinz; float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx; float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
// standard alignment, nothing to calculate if (isBoardAlignmentStandard(boardAlignment)) {
if (!mcfg.board_align_roll && !mcfg.board_align_pitch && !mcfg.board_align_yaw)
return; return;
}
standardBoardAlignment = false; standardBoardAlignment = false;
// deg2rad // deg2rad
roll = mcfg.board_align_roll * M_PI / 180.0f; roll = boardAlignment->rollDegrees * M_PI / 180.0f;
pitch = mcfg.board_align_pitch * M_PI / 180.0f; pitch = boardAlignment->pitchDegrees * M_PI / 180.0f;
yaw = mcfg.board_align_yaw * M_PI / 180.0f; yaw = boardAlignment->yawDegrees * M_PI / 180.0f;
cosx = cosf(roll); cosx = cosf(roll);
sinx = sinf(roll); sinx = sinf(roll);
@ -48,7 +61,7 @@ void initBoardAlignment(void)
boardRotation[2][2] = cosy * cosx; boardRotation[2][2] = cosy * cosx;
} }
void alignBoard(int16_t *vec) static void alignBoard(int16_t *vec)
{ {
int16_t x = vec[X]; int16_t x = vec[X];
int16_t y = vec[Y]; int16_t y = vec[Y];

View file

@ -1,4 +1,10 @@
#pragma once #pragma once
typedef struct boardAlignment_s {
int16_t rollDegrees;
int16_t pitchDegrees;
int16_t yawDegrees;
} boardAlignment_t;
void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation); void alignSensors(int16_t *src, int16_t *dest, uint8_t rotation);
void initBoardAlignment(void); void initBoardAlignment(boardAlignment_t *boardAlignment);

View file

@ -11,6 +11,7 @@
#include "flight_mixer.h" #include "flight_mixer.h"
#include "sensors_common.h" #include "sensors_common.h"
#include "battery.h" #include "battery.h"
#include "boardalignment.h"
#include "config.h" #include "config.h"
#include "drivers/serial_common.h" #include "drivers/serial_common.h"

View file

@ -187,9 +187,9 @@ static void resetConf(void)
mcfg.gyro_align = ALIGN_DEFAULT; mcfg.gyro_align = ALIGN_DEFAULT;
mcfg.acc_align = ALIGN_DEFAULT; mcfg.acc_align = ALIGN_DEFAULT;
mcfg.mag_align = ALIGN_DEFAULT; mcfg.mag_align = ALIGN_DEFAULT;
mcfg.board_align_roll = 0; mcfg.boardAlignment.rollDegrees = 0;
mcfg.board_align_pitch = 0; mcfg.boardAlignment.pitchDegrees = 0;
mcfg.board_align_yaw = 0; mcfg.boardAlignment.yawDegrees = 0;
mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect mcfg.acc_hardware = ACC_DEFAULT; // default/autodetect
mcfg.max_angle_inclination = 500; // 50 degrees mcfg.max_angle_inclination = 500; // 50 degrees
mcfg.yaw_control_direction = 1; mcfg.yaw_control_direction = 1;

View file

@ -134,9 +134,7 @@ typedef struct master_t {
sensor_align_e gyro_align; // gyro alignment sensor_align_e gyro_align; // gyro alignment
sensor_align_e acc_align; // acc alignment sensor_align_e acc_align; // acc alignment
sensor_align_e mag_align; // mag alignment sensor_align_e mag_align; // mag alignment
int16_t board_align_roll; // board alignment correction in roll (deg) boardAlignment_t boardAlignment;
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) 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 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. uint16_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen.

View file

@ -3,6 +3,8 @@
#include "rx.h" #include "rx.h"
#include "telemetry_common.h" #include "telemetry_common.h"
#include "boardalignment.h"
#include "config.h"
#include "build_config.h" #include "build_config.h"
@ -37,7 +39,7 @@ int main(void)
} }
adcInit(&adc_params); adcInit(&adc_params);
initBoardAlignment(); initBoardAlignment(&mcfg.boardAlignment);
// We have these sensors; SENSORS_SET defined in board.h depending on hardware platform // We have these sensors; SENSORS_SET defined in board.h depending on hardware platform
sensorsSet(SENSORS_SET); sensorsSet(SENSORS_SET);

View file

@ -154,9 +154,9 @@ const clivalue_t valueTable[] = {
{ "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 }, { "align_gyro", VAR_UINT8, &mcfg.gyro_align, 0, 8 },
{ "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 }, { "align_acc", VAR_UINT8, &mcfg.acc_align, 0, 8 },
{ "align_mag", VAR_UINT8, &mcfg.mag_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_roll", VAR_INT16, &mcfg.boardAlignment.rollDegrees, -180, 360 },
{ "align_board_pitch", VAR_INT16, &mcfg.board_align_pitch, -180, 360 }, { "align_board_pitch", VAR_INT16, &mcfg.boardAlignment.pitchDegrees, -180, 360 },
{ "align_board_yaw", VAR_INT16, &mcfg.board_align_yaw, -180, 360 }, { "align_board_yaw", VAR_INT16, &mcfg.boardAlignment.yawDegrees, -180, 360 },
{ "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 }, { "yaw_control_direction", VAR_INT8, &mcfg.yaw_control_direction, -1, 1 },
{ "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 }, { "acc_hardware", VAR_UINT8, &mcfg.acc_hardware, 0, 5 },
{ "max_angle_inclination", VAR_UINT16, &mcfg.max_angle_inclination, 100, 900 }, { "max_angle_inclination", VAR_UINT16, &mcfg.max_angle_inclination, 100, 900 },