rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

OP-191: Add svn:eol-style property for native line endings on checkout;

Browse Source 
Add (temporarily disabled) hooks for the rotational misalignment between the accelerometer and magnetometer.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@3151 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
jonathan 2011-04-10 18:37:32 +00:00 committed by jonathan
parent c70e134cf8
commit 9f5c393672
2 changed files with 46 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
flight/AHRS/Makefile
View File

@ -28,11 +28,11 @@ include $(TOP)/make/firmware-defs.mk
# Set developer code and compile options
# Set to YES for debugging
DEBUG ?= NO
DEBUG ?= YES
USE_BOOTLOADER ?= NO
# Set to YES when using Code Sourcery toolchain
CODE_SOURCERY ?= YES
CODE_SOURCERY ?= NO
ifeq ($(CODE_SOURCERY), YES)
REMOVE_CMD = cs-rm
@ -264,7 +264,7 @@ CFLAGS += -mcpu=$(MCU)
CFLAGS += $(CDEFS)
CFLAGS += $(patsubst %,-I%,$(EXTRAINCDIRS)) -I.
CFLAGS += -mapcs-frame
CFLAGS += -mapcs-frame -ffunction-sections
CFLAGS += -fomit-frame-pointer
ifeq ($(CODE_SOURCERY), YES)
CFLAGS += -fpromote-loop-indices

44
flight/AHRS/ahrs.c
View File

@ -95,6 +95,8 @@ void altitude_callback(AhrsObjHandle obj);
void calibration_callback(AhrsObjHandle obj);
void gps_callback(AhrsObjHandle obj);
void settings_callback(AhrsObjHandle obj);
void affine_rotate(float scale[3][4], float rotation[3]);
void calibration(float result[3], float scale[3][4], float arg[3]);
/* Bootloader related functions and var*/
static uint32_t iap_calc_crc(void);
@ -696,7 +698,7 @@ bool get_accel_gyro_data()
return true;
}
/*
/**
* @brief Perform calibration of a 3-axis field sensor using an affine transformation
* matrix.
*
@ -720,6 +722,36 @@ void calibration(float result[3], float scale[3][4], float arg[3])
}
}
/**
* @brief Scale an affine transformation matrix by a rotation, defined by a
* rotation vector. scale <- rotation * scale
*
* @param scale[in,out] The affine transformation matrix to be rotated
* @param rotation[in] The rotation vector defining the rotation
*/
void affine_rotate(float scale[3][4], float rotation[3])
{
// Rotate the scale factor matrix in-place
float rmatrix[3][3];
Rv2Rot(rotation, rmatrix);
float ret[3][4];
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
ret[row][col] = 0.0f;
for (int i = 0; i < 3; ++i) {
ret[row][col] += rmatrix[row][i] * scale[i][col];
}
}
}
// copy output to argument
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
scale[row][col] = ret[row][col];
}
}
}
/**
* @brief Downsample the analog data
* @return none
@ -1089,6 +1121,16 @@ void calibration_callback(AhrsObjHandle obj)
accel_data.calibration.scale[1][2] = cal.accel_ortho[2];
accel_data.calibration.scale[2][1] = cal.accel_ortho[2];
#if 0
// TODO: Enable after v1.0 feature freeze.
float rotation[3] = { cal.accel_rotation[0],
cal.accel_rotation[1],
cal.accel_rotation[2],
};
affine_rotate(accel_data.calibration.scale, rotation);
#endif
for(int ct=0; ct<3; ct++)
{
accel_data.calibration.scale[ct][ct] = cal.accel_scale[ct];