dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

get_rot_matrix() moved to separate library, accel calibration of rotated board fixed

This commit is contained in:
Anton Babushkin 2013-10-20 19:36:42 +02:00
parent 1a3845c66a
commit ef6f1f6f80
8 changed files with 192 additions and 106 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
makefiles/config_px4fmu-v1_default.mk
View File

@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
#
# Demo apps

1
makefiles/config_px4fmu-v2_default.mk
View File

@ -111,6 +111,7 @@ MODULES += lib/mathlib/math/filter
MODULES += lib/ecl
MODULES += lib/external_lgpl
MODULES += lib/geo
MODULES += lib/conversion
#
# Demo apps

38
src/lib/conversion/module.mk Normal file
View File

@ -0,0 +1,38 @@
############################################################################
#
# Copyright (C) 2013 PX4 Development Team. All rights reserved.
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions
# are met:
#
# 1. Redistributions of source code must retain the above copyright
# notice, this list of conditions and the following disclaimer.
# 2. Redistributions in binary form must reproduce the above copyright
# notice, this list of conditions and the following disclaimer in
# the documentation and/or other materials provided with the
# distribution.
# 3. Neither the name PX4 nor the names of its contributors may be
# used to endorse or promote products derived from this software
# without specific prior written permission.
#
# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
# POSSIBILITY OF SUCH DAMAGE.
#
############################################################################
#
# Rotation library
#
SRCS = rotation.cpp

30
src/lib/conversion/rotation.cpp Normal file
View File

@ -0,0 +1,30 @@
/*
* rotation.cpp
*
* Created on: 20.10.2013
* Author: ton
*/
#include "math.h"
#include "rotation.h"
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
{
/* first set to zero */
rot_matrix->Matrix::zero(3, 3);
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
math::EulerAngles euler(roll, pitch, yaw);
math::Dcm R(euler);
for (int i = 0; i < 3; i++) {
for (int j = 0; j < 3; j++) {
(*rot_matrix)(i, j) = R(i, j);
}
}
}

89
src/lib/conversion/rotation.h Normal file
View File

@ -0,0 +1,89 @@
/*
* rotation.h
*
* Created on: 20.10.2013
* Author: ton
*/
#ifndef ROTATION_H_
#define ROTATION_H_
#include <unistd.h>
#include <mathlib/mathlib.h>
/**
* Enum for board and external compass rotations.
* This enum maps from board attitude to airframe attitude.
*/
enum Rotation {
ROTATION_NONE = 0,
ROTATION_YAW_45 = 1,
ROTATION_YAW_90 = 2,
ROTATION_YAW_135 = 3,
ROTATION_YAW_180 = 4,
ROTATION_YAW_225 = 5,
ROTATION_YAW_270 = 6,
ROTATION_YAW_315 = 7,
ROTATION_ROLL_180 = 8,
ROTATION_ROLL_180_YAW_45 = 9,
ROTATION_ROLL_180_YAW_90 = 10,
ROTATION_ROLL_180_YAW_135 = 11,
ROTATION_PITCH_180 = 12,
ROTATION_ROLL_180_YAW_225 = 13,
ROTATION_ROLL_180_YAW_270 = 14,
ROTATION_ROLL_180_YAW_315 = 15,
ROTATION_ROLL_90 = 16,
ROTATION_ROLL_90_YAW_45 = 17,
ROTATION_ROLL_90_YAW_90 = 18,
ROTATION_ROLL_90_YAW_135 = 19,
ROTATION_ROLL_270 = 20,
ROTATION_ROLL_270_YAW_45 = 21,
ROTATION_ROLL_270_YAW_90 = 22,
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_MAX
};
typedef struct {
uint16_t roll;
uint16_t pitch;
uint16_t yaw;
} rot_lookup_t;
const rot_lookup_t rot_lookup[] = {
{ 0, 0, 0 },
{ 0, 0, 45 },
{ 0, 0, 90 },
{ 0, 0, 135 },
{ 0, 0, 180 },
{ 0, 0, 225 },
{ 0, 0, 270 },
{ 0, 0, 315 },
{180, 0, 0 },
{180, 0, 45 },
{180, 0, 90 },
{180, 0, 135 },
{ 0, 180, 0 },
{180, 0, 225 },
{180, 0, 270 },
{180, 0, 315 },
{ 90, 0, 0 },
{ 90, 0, 45 },
{ 90, 0, 90 },
{ 90, 0, 135 },
{270, 0, 0 },
{270, 0, 45 },
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
};
/**
* Get the rotation matrix
*/
__EXPORT void
get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
#endif /* ROTATION_H_ */

45
src/modules/commander/accelerometer_calibration.cpp
View File

@ -112,11 +112,13 @@
#include <fcntl.h>
#include <sys/prctl.h>
#include <math.h>
#include <mathlib/mathlib.h>
#include <string.h>
#include <drivers/drv_hrt.h>
#include <uORB/topics/sensor_combined.h>
#include <drivers/drv_accel.h>
#include <geo/geo.h>
#include <conversion/rotation.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <mavlink/mavlink_log.h>
@ -144,24 +146,41 @@ int do_accel_calibration(int mavlink_fd) {
int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
if (res == OK) {
/* measurements complete successfully, set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
/* measurements complete successfully, rotate calibration values */
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
enum Rotation board_rotation_id;
param_get(board_rotation_h, &(board_rotation_id));
math::Matrix board_rotation(3, 3);
get_rot_matrix(board_rotation_id, &board_rotation);
board_rotation = board_rotation.transpose();
math::Vector3 vect(3);
vect(0) = accel_offs[0];
vect(1) = accel_offs[1];
vect(2) = accel_offs[2];
math::Vector3 accel_offs_rotated = board_rotation * vect;
vect(0) = accel_scale[0];
vect(1) = accel_scale[1];
vect(2) = accel_scale[2];
math::Vector3 accel_scale_rotated = board_rotation * vect;
/* set parameters */
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0)))
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0)))
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1)))
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) {
mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
}
int fd = open(ACCEL_DEVICE_PATH, 0);
struct accel_scale ascale = {
accel_offs[0],
accel_scale[0],
accel_offs[1],
accel_scale[1],
accel_offs[2],
accel_scale[2],
accel_offs_rotated(0),
accel_scale_rotated(0),
accel_offs_rotated(1),
accel_scale_rotated(1),
accel_offs_rotated(2),
accel_scale_rotated(2),
};
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))

1
src/modules/commander/module.mk
View File

@ -47,4 +47,3 @@ SRCS = commander.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp

93
src/modules/sensors/sensors.cpp
View File

@ -67,6 +67,7 @@
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/perf_counter.h>
#include <conversion/rotation.h>
#include <systemlib/airspeed.h>
@ -135,75 +136,6 @@
#define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
/**
* Enum for board and external compass rotations.
* This enum maps from board attitude to airframe attitude.
*/
enum Rotation {
ROTATION_NONE = 0,
ROTATION_YAW_45 = 1,
ROTATION_YAW_90 = 2,
ROTATION_YAW_135 = 3,
ROTATION_YAW_180 = 4,
ROTATION_YAW_225 = 5,
ROTATION_YAW_270 = 6,
ROTATION_YAW_315 = 7,
ROTATION_ROLL_180 = 8,
ROTATION_ROLL_180_YAW_45 = 9,
ROTATION_ROLL_180_YAW_90 = 10,
ROTATION_ROLL_180_YAW_135 = 11,
ROTATION_PITCH_180 = 12,
ROTATION_ROLL_180_YAW_225 = 13,
ROTATION_ROLL_180_YAW_270 = 14,
ROTATION_ROLL_180_YAW_315 = 15,
ROTATION_ROLL_90 = 16,
ROTATION_ROLL_90_YAW_45 = 17,
ROTATION_ROLL_90_YAW_90 = 18,
ROTATION_ROLL_90_YAW_135 = 19,
ROTATION_ROLL_270 = 20,
ROTATION_ROLL_270_YAW_45 = 21,
ROTATION_ROLL_270_YAW_90 = 22,
ROTATION_ROLL_270_YAW_135 = 23,
ROTATION_PITCH_90 = 24,
ROTATION_PITCH_270 = 25,
ROTATION_MAX
};
typedef struct {
uint16_t roll;
uint16_t pitch;
uint16_t yaw;
} rot_lookup_t;
const rot_lookup_t rot_lookup[] = {
{ 0, 0, 0 },
{ 0, 0, 45 },
{ 0, 0, 90 },
{ 0, 0, 135 },
{ 0, 0, 180 },
{ 0, 0, 225 },
{ 0, 0, 270 },
{ 0, 0, 315 },
{180, 0, 0 },
{180, 0, 45 },
{180, 0, 90 },
{180, 0, 135 },
{ 0, 180, 0 },
{180, 0, 225 },
{180, 0, 270 },
{180, 0, 315 },
{ 90, 0, 0 },
{ 90, 0, 45 },
{ 90, 0, 90 },
{ 90, 0, 135 },
{270, 0, 0 },
{270, 0, 45 },
{270, 0, 90 },
{270, 0, 135 },
{ 0, 90, 0 },
{ 0, 270, 0 }
};
/**
* Sensor app start / stop handling function
*
@ -384,11 +316,6 @@ private:
*/
int parameters_update();
/**
* Get the rotation matrices
*/
void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
/**
* Do accel-related initialisation.
*/
@ -803,24 +730,6 @@ Sensors::parameters_update()
return OK;
}
void
Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
{
/* first set to zero */
rot_matrix->Matrix::zero(3, 3);
float roll = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
float yaw = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
math::EulerAngles euler(roll, pitch, yaw);
math::Dcm R(euler);
for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
(*rot_matrix)(i, j) = R(i, j);
}
void
Sensors::accel_init()
{