forked from Archive/PX4-Autopilot
Merge pull request #483 from PX4/calib_rotation
Calibration of rotated board
This commit is contained in:
commit
17ddc7f471
|
@ -115,6 +115,7 @@ MODULES += lib/mathlib/math/filter
|
|||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
|
|
|
@ -112,6 +112,7 @@ MODULES += lib/mathlib/math/filter
|
|||
MODULES += lib/ecl
|
||||
MODULES += lib/external_lgpl
|
||||
MODULES += lib/geo
|
||||
MODULES += lib/conversion
|
||||
|
||||
#
|
||||
# Demo apps
|
||||
|
|
|
@ -66,7 +66,7 @@ struct accel_report {
|
|||
int16_t temperature_raw;
|
||||
};
|
||||
|
||||
/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
|
||||
/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
|
||||
struct accel_scale {
|
||||
float x_offset;
|
||||
float x_scale;
|
||||
|
|
|
@ -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.
|
||||
#
|
||||
############################################################################
|
||||
|
||||
#
|
||||
# Conversion library
|
||||
#
|
||||
|
||||
SRCS = rotation.cpp
|
|
@ -0,0 +1,62 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rotation.cpp
|
||||
*
|
||||
* Vector rotation library
|
||||
*/
|
||||
|
||||
#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);
|
||||
}
|
||||
}
|
||||
}
|
|
@ -0,0 +1,121 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file rotation.h
|
||||
*
|
||||
* Vector rotation library
|
||||
*/
|
||||
|
||||
#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_ */
|
|
@ -100,10 +100,29 @@
|
|||
* accel_T = A^-1 * g
|
||||
* g = 9.80665
|
||||
*
|
||||
* ===== Rotation =====
|
||||
*
|
||||
* Calibrating using model:
|
||||
* accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
|
||||
*
|
||||
* Actual correction:
|
||||
* accel_corr = rot * accel_T * (accel_raw - accel_offs)
|
||||
*
|
||||
* Known: accel_T_r, accel_offs_r, rot
|
||||
* Unknown: accel_T, accel_offs
|
||||
*
|
||||
* Solution:
|
||||
* accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
|
||||
* rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
|
||||
* rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
|
||||
* => accel_T = rot^-1 * accel_T_r * rot
|
||||
* => accel_offs = rot^-1 * accel_offs_r
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#include "accelerometer_calibration.h"
|
||||
#include "calibration_messages.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <unistd.h>
|
||||
|
@ -112,11 +131,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>
|
||||
|
@ -127,75 +148,19 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
|
||||
static const char *sensor_name = "accel";
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined);
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
|
||||
int mat_invert3(float src[3][3], float dst[3][3]);
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
|
||||
|
||||
int do_accel_calibration(int mavlink_fd) {
|
||||
/* announce change */
|
||||
mavlink_log_info(mavlink_fd, "accel calibration started");
|
||||
mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
|
||||
int do_accel_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
|
||||
/* measure and calculate offsets & scales */
|
||||
float accel_offs[3];
|
||||
float accel_scale[3];
|
||||
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]))) {
|
||||
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],
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
|
||||
warn("WARNING: failed to set scale / offsets for accel");
|
||||
|
||||
close(fd);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "accel calibration done");
|
||||
return OK;
|
||||
} else {
|
||||
/* measurements error */
|
||||
mavlink_log_info(mavlink_fd, "accel calibration aborted");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* exit accel calibration mode */
|
||||
}
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
|
||||
const int samples_num = 2500;
|
||||
float accel_ref[6][3];
|
||||
bool data_collected[6] = { false, false, false, false, false, false };
|
||||
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
|
||||
|
||||
/* reset existing calibration */
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
struct accel_scale ascale_null = {
|
||||
struct accel_scale accel_scale = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
0.0f,
|
||||
|
@ -203,17 +168,102 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
0.0f,
|
||||
1.0f,
|
||||
};
|
||||
int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
|
||||
|
||||
int res = OK;
|
||||
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
|
||||
if (OK != ioctl_res) {
|
||||
warn("ERROR: failed to set scale / offsets for accel");
|
||||
return ERROR;
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
float accel_offs[3];
|
||||
float accel_T[3][3];
|
||||
|
||||
if (res == OK) {
|
||||
/* measure and calculate offsets & scales */
|
||||
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* measurements completed successfully, rotate calibration values */
|
||||
param_t board_rotation_h = param_find("SENS_BOARD_ROT");
|
||||
int32_t board_rotation_int;
|
||||
param_get(board_rotation_h, &(board_rotation_int));
|
||||
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
|
||||
math::Matrix board_rotation(3, 3);
|
||||
get_rot_matrix(board_rotation_id, &board_rotation);
|
||||
math::Matrix board_rotation_t = board_rotation.transpose();
|
||||
math::Vector3 accel_offs_vec;
|
||||
accel_offs_vec.set(&accel_offs[0]);
|
||||
math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
|
||||
math::Matrix accel_T_mat(3, 3);
|
||||
accel_T_mat.set(&accel_T[0][0]);
|
||||
math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
|
||||
|
||||
accel_scale.x_offset = accel_offs_rotated(0);
|
||||
accel_scale.x_scale = accel_T_rotated(0, 0);
|
||||
accel_scale.y_offset = accel_offs_rotated(1);
|
||||
accel_scale.y_scale = accel_T_rotated(1, 1);
|
||||
accel_scale.z_offset = accel_offs_rotated(2);
|
||||
accel_scale.z_scale = accel_T_rotated(2, 2);
|
||||
|
||||
/* set parameters */
|
||||
if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
|
||||
|| param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
|
||||
|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
|
||||
|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
|
||||
|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
|
||||
|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* apply new scaling and offsets */
|
||||
int fd = open(ACCEL_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
res = param_save_default();
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
|
||||
{
|
||||
const int samples_num = 2500;
|
||||
float accel_ref[6][3];
|
||||
bool data_collected[6] = { false, false, false, false, false, false };
|
||||
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
|
||||
|
||||
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
|
||||
|
||||
unsigned done_count = 0;
|
||||
int res = OK;
|
||||
|
||||
while (true) {
|
||||
bool done = true;
|
||||
|
@ -221,64 +271,63 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
done_count = 0;
|
||||
|
||||
for (int i = 0; i < 6; i++) {
|
||||
if (!data_collected[i]) {
|
||||
if (data_collected[i]) {
|
||||
done_count++;
|
||||
|
||||
} else {
|
||||
done = false;
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
|
||||
(!data_collected[0]) ? "x+ " : "",
|
||||
(!data_collected[1]) ? "x- " : "",
|
||||
(!data_collected[2]) ? "y+ " : "",
|
||||
(!data_collected[3]) ? "y- " : "",
|
||||
(!data_collected[4]) ? "z+ " : "",
|
||||
(!data_collected[5]) ? "z- " : "");
|
||||
|
||||
if (old_done_count != done_count)
|
||||
mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
|
||||
|
||||
if (done)
|
||||
break;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
|
||||
(!data_collected[0]) ? "x+ " : "",
|
||||
(!data_collected[1]) ? "x- " : "",
|
||||
(!data_collected[2]) ? "y+ " : "",
|
||||
(!data_collected[3]) ? "y- " : "",
|
||||
(!data_collected[4]) ? "z+ " : "",
|
||||
(!data_collected[5]) ? "z- " : "");
|
||||
|
||||
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
|
||||
|
||||
if (orient < 0) {
|
||||
close(sensor_combined_sub);
|
||||
return ERROR;
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
if (data_collected[orient]) {
|
||||
mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
|
||||
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
|
||||
continue;
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
|
||||
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
|
||||
mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
|
||||
(double)accel_ref[orient][0],
|
||||
(double)accel_ref[orient][1],
|
||||
(double)accel_ref[orient][2]);
|
||||
(double)accel_ref[orient][0],
|
||||
(double)accel_ref[orient][1],
|
||||
(double)accel_ref[orient][2]);
|
||||
|
||||
data_collected[orient] = true;
|
||||
tune_neutral();
|
||||
}
|
||||
|
||||
close(sensor_combined_sub);
|
||||
|
||||
/* calculate offsets and rotation+scale matrix */
|
||||
float accel_T[3][3];
|
||||
int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
|
||||
if (res != 0) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
|
||||
return ERROR;
|
||||
if (res == OK) {
|
||||
/* calculate offsets and transform matrix */
|
||||
res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
|
||||
}
|
||||
}
|
||||
|
||||
/* convert accel transform matrix to scales,
|
||||
* rotation part of transform matrix is not used by now
|
||||
*/
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_scale[i] = accel_T[i][i];
|
||||
}
|
||||
|
||||
return OK;
|
||||
return res;
|
||||
}
|
||||
|
||||
/*
|
||||
|
@ -287,14 +336,15 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
|
|||
* @return 0..5 according to orientation when vehicle is still and ready for measurements,
|
||||
* ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
|
||||
*/
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
|
||||
int detect_orientation(int mavlink_fd, int sub_sensor_combined)
|
||||
{
|
||||
struct sensor_combined_s sensor;
|
||||
/* exponential moving average of accel */
|
||||
float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
|
||||
/* max-hold dispersion of accel */
|
||||
float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
|
||||
/* EMA time constant in seconds*/
|
||||
float ema_len = 0.2f;
|
||||
float ema_len = 0.5f;
|
||||
/* set "still" threshold to 0.25 m/s^2 */
|
||||
float still_thr2 = pow(0.25f, 2);
|
||||
/* set accel error threshold to 5m/s^2 */
|
||||
|
@ -318,30 +368,38 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
|
|||
while (true) {
|
||||
/* wait blocking for new data */
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret) {
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
|
||||
t = hrt_absolute_time();
|
||||
float dt = (t - t_prev) / 1000000.0f;
|
||||
t_prev = t;
|
||||
float w = dt / ema_len;
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
|
||||
float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
|
||||
float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
|
||||
accel_ema[i] += d * w;
|
||||
d = d * d;
|
||||
accel_disp[i] = accel_disp[i] * (1.0f - w);
|
||||
|
||||
if (d > still_thr2 * 8.0f)
|
||||
d = still_thr2 * 8.0f;
|
||||
|
||||
if (d > accel_disp[i])
|
||||
accel_disp[i] = d;
|
||||
}
|
||||
|
||||
/* still detector with hysteresis */
|
||||
if ( accel_disp[0] < still_thr2 &&
|
||||
accel_disp[1] < still_thr2 &&
|
||||
accel_disp[2] < still_thr2 ) {
|
||||
if (accel_disp[0] < still_thr2 &&
|
||||
accel_disp[1] < still_thr2 &&
|
||||
accel_disp[2] < still_thr2) {
|
||||
/* is still now */
|
||||
if (t_still == 0) {
|
||||
/* first time */
|
||||
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
|
||||
t_still = t;
|
||||
t_timeout = t + timeout;
|
||||
|
||||
} else {
|
||||
/* still since t_still */
|
||||
if (t > t_still + still_time) {
|
||||
|
@ -349,62 +407,71 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
|
|||
break;
|
||||
}
|
||||
}
|
||||
} else if ( accel_disp[0] > still_thr2 * 2.0f ||
|
||||
accel_disp[1] > still_thr2 * 2.0f ||
|
||||
accel_disp[2] > still_thr2 * 2.0f) {
|
||||
|
||||
} else if (accel_disp[0] > still_thr2 * 4.0f ||
|
||||
accel_disp[1] > still_thr2 * 4.0f ||
|
||||
accel_disp[2] > still_thr2 * 4.0f) {
|
||||
/* not still, reset still start time */
|
||||
if (t_still != 0) {
|
||||
mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
|
||||
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
|
||||
t_still = 0;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (poll_ret == 0) {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (t > t_timeout) {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
|
||||
return -1;
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 0; // [ g, 0, 0 ]
|
||||
if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
|
||||
if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 1; // [ -g, 0, 0 ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 2; // [ 0, g, 0 ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr )
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
|
||||
fabsf(accel_ema[2]) < accel_err_thr)
|
||||
return 3; // [ 0, -g, 0 ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr )
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
|
||||
return 4; // [ 0, 0, g ]
|
||||
if ( fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr )
|
||||
|
||||
if (fabsf(accel_ema[0]) < accel_err_thr &&
|
||||
fabsf(accel_ema[1]) < accel_err_thr &&
|
||||
fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
|
||||
return 5; // [ 0, 0, -g ]
|
||||
|
||||
mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
|
||||
|
||||
return -2; // Can't detect orientation
|
||||
return ERROR; // Can't detect orientation
|
||||
}
|
||||
|
||||
/*
|
||||
* Read specified number of accelerometer samples, calculate average and dispersion.
|
||||
*/
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
|
||||
int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num)
|
||||
{
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sensor_combined_sub;
|
||||
fds[0].events = POLLIN;
|
||||
|
@ -415,12 +482,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
|
|||
|
||||
while (count < samples_num) {
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret == 1) {
|
||||
struct sensor_combined_s sensor;
|
||||
orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
|
||||
|
||||
for (int i = 0; i < 3; i++)
|
||||
accel_sum[i] += sensor.accelerometer_m_s2[i];
|
||||
|
||||
count++;
|
||||
|
||||
} else {
|
||||
errcount++;
|
||||
continue;
|
||||
|
@ -437,10 +508,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
|
|||
return OK;
|
||||
}
|
||||
|
||||
int mat_invert3(float src[3][3], float dst[3][3]) {
|
||||
int mat_invert3(float src[3][3], float dst[3][3])
|
||||
{
|
||||
float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
|
||||
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
|
||||
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
|
||||
src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
|
||||
src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
|
||||
|
||||
if (det == 0.0f)
|
||||
return ERROR; // Singular matrix
|
||||
|
||||
|
@ -457,7 +530,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
|
|||
return OK;
|
||||
}
|
||||
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
|
||||
int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g)
|
||||
{
|
||||
/* calculate offsets */
|
||||
for (int i = 0; i < 3; i++) {
|
||||
accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
|
||||
|
@ -466,6 +540,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
|
|||
/* fill matrix A for linear equations system*/
|
||||
float mat_A[3][3];
|
||||
memset(mat_A, 0, sizeof(mat_A));
|
||||
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int j = 0; j < 3; j++) {
|
||||
float a = accel_ref[i * 2][j] - accel_offs[j];
|
||||
|
@ -475,6 +550,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
|
|||
|
||||
/* calculate inverse matrix for A */
|
||||
float mat_A_inv[3][3];
|
||||
|
||||
if (mat_invert3(mat_A, mat_A_inv) != OK)
|
||||
return ERROR;
|
||||
|
||||
|
|
|
@ -0,0 +1,57 @@
|
|||
/****************************************************************************
|
||||
*
|
||||
* Copyright (C) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: Anton Babushkin <anton.babushkin@me.com>
|
||||
*
|
||||
* 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.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file calibration_messages.h
|
||||
*
|
||||
* Common calibration messages.
|
||||
*
|
||||
* @author Anton Babushkin <anton.babushkin@me.com>
|
||||
*/
|
||||
|
||||
#ifndef CALIBRATION_MESSAGES_H_
|
||||
#define CALIBRATION_MESSAGES_H_
|
||||
|
||||
#define CAL_STARTED_MSG "%s calibration: started"
|
||||
#define CAL_DONE_MSG "%s calibration: done"
|
||||
#define CAL_FAILED_MSG "%s calibration: failed"
|
||||
#define CAL_PROGRESS_MSG "%s calibration: progress <%u>"
|
||||
|
||||
#define CAL_FAILED_SENSOR_MSG "ERROR: failed reading sensor"
|
||||
#define CAL_FAILED_RESET_CAL_MSG "ERROR: failed to reset calibration"
|
||||
#define CAL_FAILED_APPLY_CAL_MSG "ERROR: failed to apply calibration"
|
||||
#define CAL_FAILED_SET_PARAMS_MSG "ERROR: failed to set parameters"
|
||||
#define CAL_FAILED_SAVE_PARAMS_MSG "ERROR: failed to save parameters"
|
||||
|
||||
#endif /* CALIBRATION_MESSAGES_H_ */
|
|
@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
|
||||
/* reset the arming mode to disarmed */
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
|
||||
|
||||
if (arming_res != TRANSITION_DENIED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
|
||||
}
|
||||
|
@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
|
|||
break;
|
||||
}
|
||||
|
||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM:
|
||||
{
|
||||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
|
||||
if (safety->safety_switch_available && !safety->safety_off) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
arming_res = TRANSITION_DENIED;
|
||||
case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
|
||||
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
} else {
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
|
||||
}
|
||||
if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
|
||||
if (safety->safety_switch_available && !safety->safety_off) {
|
||||
print_reject_arm("NOT ARMING: Press safety switch first.");
|
||||
arming_res = TRANSITION_DENIED;
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
|
||||
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
} else {
|
||||
arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
|
||||
}
|
||||
|
||||
if (arming_res == TRANSITION_CHANGED) {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
|
||||
result = VEHICLE_CMD_RESULT_ACCEPTED;
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
|
||||
result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
|
@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
last_idle_time = system_load.tasks[0].total_runtime;
|
||||
|
||||
/* check if board is connected via USB */
|
||||
struct stat statbuf;
|
||||
//struct stat statbuf;
|
||||
//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
|
||||
}
|
||||
|
||||
|
@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
|
||||
if (armed.armed) {
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
|
||||
|
||||
} else {
|
||||
arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
|
||||
}
|
||||
|
@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[])
|
|||
counter++;
|
||||
|
||||
int blink_state = blink_msg_state();
|
||||
|
||||
if (blink_state > 0) {
|
||||
/* blinking LED message, don't touch LEDs */
|
||||
if (blink_state == 2) {
|
||||
/* blinking LED message completed, restore normal state */
|
||||
control_status_leds(&status, &armed, true);
|
||||
}
|
||||
|
||||
} else {
|
||||
/* normal state */
|
||||
control_status_leds(&status, &armed, status_changed);
|
||||
|
@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
|
|||
ret = pthread_join(commander_low_prio_thread, NULL);
|
||||
|
||||
if (ret) {
|
||||
warn("join failed", ret);
|
||||
warn("join failed: %d", ret);
|
||||
}
|
||||
|
||||
rgbled_set_mode(RGBLED_MODE_OFF);
|
||||
|
@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
|
|||
/* driving rgbled */
|
||||
if (changed) {
|
||||
bool set_normal_color = false;
|
||||
|
||||
/* set mode */
|
||||
if (status->arming_state == ARMING_STATE_ARMED) {
|
||||
rgbled_set_mode(RGBLED_MODE_ON);
|
||||
|
@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
|
|||
if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
|
||||
rgbled_set_color(RGBLED_COLOR_AMBER);
|
||||
}
|
||||
|
||||
/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
|
||||
|
||||
} else {
|
||||
|
@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg)
|
|||
fds[0].events = POLLIN;
|
||||
|
||||
while (!thread_should_exit) {
|
||||
|
||||
/* wait for up to 100ms for data */
|
||||
/* wait for up to 200ms for data */
|
||||
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
|
||||
|
||||
/* timed out - periodic check for _task_should_exit, etc. */
|
||||
/* timed out - periodic check for thread_should_exit, etc. */
|
||||
if (pret == 0)
|
||||
continue;
|
||||
|
||||
|
@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg)
|
|||
|
||||
} else if ((int)(cmd.param4) == 1) {
|
||||
/* RC calibration */
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
|
||||
answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
|
||||
calib_ret = do_rc_calibration(mavlink_fd);
|
||||
|
||||
} else if ((int)(cmd.param5) == 1) {
|
||||
|
@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg)
|
|||
/* send acknowledge command */
|
||||
// XXX TODO
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
close(cmd_sub);
|
||||
|
|
|
@ -33,10 +33,12 @@
|
|||
|
||||
/**
|
||||
* @file gyro_calibration.cpp
|
||||
*
|
||||
* Gyroscope calibration routine
|
||||
*/
|
||||
|
||||
#include "gyro_calibration.h"
|
||||
#include "calibration_messages.h"
|
||||
#include "commander_helper.h"
|
||||
|
||||
#include <stdio.h>
|
||||
|
@ -56,9 +58,12 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
static const char *sensor_name = "gyro";
|
||||
|
||||
int do_gyro_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
|
||||
struct gyro_scale gyro_scale = {
|
||||
0.0f,
|
||||
|
@ -69,79 +74,87 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
1.0f,
|
||||
};
|
||||
|
||||
/* subscribe to gyro sensor topic */
|
||||
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
|
||||
struct gyro_report gyro_report;
|
||||
int res = OK;
|
||||
|
||||
/* reset all offsets to zero and all scales to one */
|
||||
int fd = open(GYRO_DEVICE_PATH, 0);
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
|
||||
warn("WARNING: failed to reset scale / offsets for gyro");
|
||||
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
|
||||
|
||||
/*** --- OFFSETS --- ***/
|
||||
|
||||
/* determine gyro mean values */
|
||||
const unsigned calibration_count = 5000;
|
||||
unsigned calibration_counter = 0;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_sensor_gyro;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
|
||||
gyro_scale.x_offset += gyro_report.x;
|
||||
gyro_scale.y_offset += gyro_report.y;
|
||||
gyro_scale.z_offset += gyro_report.z;
|
||||
calibration_counter++;
|
||||
if (calibration_counter % (calibration_count / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
|
||||
close(sub_sensor_gyro);
|
||||
return ERROR;
|
||||
}
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
gyro_scale.x_offset /= calibration_count;
|
||||
gyro_scale.y_offset /= calibration_count;
|
||||
gyro_scale.z_offset /= calibration_count;
|
||||
if (res == OK) {
|
||||
/* determine gyro mean values */
|
||||
const unsigned calibration_count = 5000;
|
||||
unsigned calibration_counter = 0;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
/* subscribe to gyro sensor topic */
|
||||
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
|
||||
struct gyro_report gyro_report;
|
||||
|
||||
while (calibration_counter < calibration_count) {
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_sensor_gyro;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
|
||||
gyro_scale.x_offset += gyro_report.x;
|
||||
gyro_scale.y_offset += gyro_report.y;
|
||||
gyro_scale.z_offset += gyro_report.z;
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_count / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
|
||||
mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)");
|
||||
close(sub_sensor_gyro);
|
||||
return ERROR;
|
||||
|
||||
gyro_scale.x_offset /= calibration_count;
|
||||
gyro_scale.y_offset /= calibration_count;
|
||||
gyro_scale.z_offset /= calibration_count;
|
||||
}
|
||||
|
||||
/* beep on calibration end */
|
||||
mavlink_log_info(mavlink_fd, "offset calibration done.");
|
||||
if (res == OK) {
|
||||
/* check offsets */
|
||||
if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
/* set offset parameters to new values */
|
||||
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|
||||
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|
||||
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
#if 0
|
||||
/* beep on offset calibration end */
|
||||
mavlink_log_info(mavlink_fd, "gyro offset calibration done");
|
||||
tune_neutral();
|
||||
|
||||
/* set offset parameters to new values */
|
||||
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
|
||||
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
|
||||
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!");
|
||||
}
|
||||
|
||||
|
||||
/*** --- SCALING --- ***/
|
||||
#if 0
|
||||
/* scale calibration */
|
||||
/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
|
||||
|
||||
mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
|
||||
|
@ -163,9 +176,11 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
// XXX change to mag topic
|
||||
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
|
||||
|
||||
float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
|
||||
if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
|
||||
if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
|
||||
float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
|
||||
|
||||
if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
|
||||
|
||||
if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
|
||||
|
||||
|
||||
uint64_t last_time = hrt_absolute_time();
|
||||
|
@ -175,7 +190,7 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
|
||||
/* abort this loop if not rotated more than 180 degrees within 5 seconds */
|
||||
if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
|
||||
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
|
||||
&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
|
||||
mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
|
||||
close(sub_sensor_combined);
|
||||
return OK;
|
||||
|
@ -203,14 +218,17 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
// calculate error between estimate and measurement
|
||||
// apply declination correction for true heading as well.
|
||||
//float mag = -atan2f(magNav(1),magNav(0));
|
||||
float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
|
||||
if (mag > M_PI_F) mag -= 2*M_PI_F;
|
||||
if (mag < -M_PI_F) mag += 2*M_PI_F;
|
||||
float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
|
||||
|
||||
if (mag > M_PI_F) mag -= 2 * M_PI_F;
|
||||
|
||||
if (mag < -M_PI_F) mag += 2 * M_PI_F;
|
||||
|
||||
float diff = mag - mag_last;
|
||||
|
||||
if (diff > M_PI_F) diff -= 2*M_PI_F;
|
||||
if (diff < -M_PI_F) diff += 2*M_PI_F;
|
||||
if (diff > M_PI_F) diff -= 2 * M_PI_F;
|
||||
|
||||
if (diff < -M_PI_F) diff += 2 * M_PI_F;
|
||||
|
||||
baseline_integral += diff;
|
||||
mag_last = mag;
|
||||
|
@ -220,15 +238,15 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
|
||||
// warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
|
||||
|
||||
// } else if (poll_ret == 0) {
|
||||
// /* any poll failure for 1s is a reason to abort */
|
||||
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
|
||||
// return;
|
||||
// } else if (poll_ret == 0) {
|
||||
// /* any poll failure for 1s is a reason to abort */
|
||||
// mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
|
||||
// return;
|
||||
}
|
||||
}
|
||||
|
||||
float gyro_scale = baseline_integral / gyro_integral;
|
||||
|
||||
|
||||
warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
|
||||
mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
|
||||
|
||||
|
@ -236,40 +254,52 @@ int do_gyro_calibration(int mavlink_fd)
|
|||
if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
|
||||
mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
|
||||
close(sub_sensor_gyro);
|
||||
mavlink_log_critical(mavlink_fd, "gyro calibration failed");
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
/* beep on calibration end */
|
||||
mavlink_log_info(mavlink_fd, "scale calibration done.");
|
||||
mavlink_log_info(mavlink_fd, "gyro scale calibration done");
|
||||
tune_neutral();
|
||||
|
||||
#endif
|
||||
|
||||
/* set scale parameters to new values */
|
||||
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|
||||
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|
||||
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
|
||||
mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!");
|
||||
if (res == OK) {
|
||||
/* set scale parameters to new values */
|
||||
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
|
||||
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
|
||||
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
/* apply new scaling and offsets */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
if (res == OK) {
|
||||
/* apply new scaling and offsets */
|
||||
fd = open(GYRO_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
|
||||
close(fd);
|
||||
|
||||
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
|
||||
warn("WARNING: failed to apply new scale for gyro");
|
||||
|
||||
close(fd);
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warnx("WARNING: auto-save of params to storage failed");
|
||||
mavlink_log_critical(mavlink_fd, "gyro store failed");
|
||||
close(sub_sensor_gyro);
|
||||
return ERROR;
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
close(sub_sensor_gyro);
|
||||
return OK;
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
res = param_save_default();
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (res == OK) {
|
||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
return res;
|
||||
}
|
||||
|
|
|
@ -33,12 +33,14 @@
|
|||
|
||||
/**
|
||||
* @file mag_calibration.cpp
|
||||
*
|
||||
* Magnetometer calibration routine
|
||||
*/
|
||||
|
||||
#include "mag_calibration.h"
|
||||
#include "commander_helper.h"
|
||||
#include "calibration_routines.h"
|
||||
#include "calibration_messages.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
|
@ -59,26 +61,20 @@
|
|||
#endif
|
||||
static const int ERROR = -1;
|
||||
|
||||
static const char *sensor_name = "mag";
|
||||
|
||||
int do_mag_calibration(int mavlink_fd)
|
||||
{
|
||||
mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
|
||||
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||
struct mag_report mag;
|
||||
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_log_info(mavlink_fd, "don't move system");
|
||||
|
||||
/* 45 seconds */
|
||||
uint64_t calibration_interval = 45 * 1000 * 1000;
|
||||
|
||||
/* maximum 2000 values */
|
||||
/* maximum 500 values */
|
||||
const unsigned int calibration_maxcount = 500;
|
||||
unsigned int calibration_counter = 0;
|
||||
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
|
||||
/* erase old calibration */
|
||||
struct mag_scale mscale_null = {
|
||||
0.0f,
|
||||
1.0f,
|
||||
|
@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd)
|
|||
1.0f,
|
||||
};
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
|
||||
int res = OK;
|
||||
|
||||
/* erase old calibration */
|
||||
int fd = open(MAG_DEVICE_PATH, O_RDONLY);
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
|
||||
}
|
||||
|
||||
/* calibrate range */
|
||||
if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
|
||||
warnx("failed to calibrate scale");
|
||||
if (res == OK) {
|
||||
/* calibrate range */
|
||||
res = ioctl(fd, MAGIOCCALIBRATE, fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
|
||||
float *x;
|
||||
float *y;
|
||||
float *z;
|
||||
|
||||
/* calibrate offsets */
|
||||
if (res == OK) {
|
||||
/* allocate memory */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
|
||||
|
||||
// uint64_t calibration_start = hrt_absolute_time();
|
||||
x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
|
||||
uint64_t axis_deadline = hrt_absolute_time();
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
|
||||
const char axislabels[3] = { 'X', 'Y', 'Z'};
|
||||
int axis_index = -1;
|
||||
|
||||
float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
|
||||
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
warnx("mag cal failed: out of memory");
|
||||
mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
|
||||
warnx("x:%p y:%p z:%p\n", x, y, z);
|
||||
return ERROR;
|
||||
if (x == NULL || y == NULL || z == NULL) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
|
||||
if (res == OK) {
|
||||
int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
|
||||
struct mag_report mag;
|
||||
|
||||
unsigned poll_errcount = 0;
|
||||
/* limit update rate to get equally spaced measurements over time (in ms) */
|
||||
orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
|
||||
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
/* calibrate offsets */
|
||||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
unsigned poll_errcount = 0;
|
||||
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_mag;
|
||||
fds[0].events = POLLIN;
|
||||
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
|
||||
|
||||
/* user guidance */
|
||||
if (hrt_absolute_time() >= axis_deadline &&
|
||||
axis_index < 3) {
|
||||
while (hrt_absolute_time() < calibration_deadline &&
|
||||
calibration_counter < calibration_maxcount) {
|
||||
|
||||
axis_index++;
|
||||
/* wait blocking for new data */
|
||||
struct pollfd fds[1];
|
||||
fds[0].fd = sub_mag;
|
||||
fds[0].events = POLLIN;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
|
||||
tune_neutral();
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
axis_deadline += calibration_interval / 3;
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
|
||||
|
||||
x[calibration_counter] = mag.x;
|
||||
y[calibration_counter] = mag.y;
|
||||
z[calibration_counter] = mag.z;
|
||||
|
||||
calibration_counter++;
|
||||
|
||||
if (calibration_counter % (calibration_maxcount / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
if (!(axis_index < 3)) {
|
||||
break;
|
||||
}
|
||||
|
||||
int poll_ret = poll(fds, 1, 1000);
|
||||
|
||||
if (poll_ret > 0) {
|
||||
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
|
||||
|
||||
x[calibration_counter] = mag.x;
|
||||
y[calibration_counter] = mag.y;
|
||||
z[calibration_counter] = mag.z;
|
||||
|
||||
calibration_counter++;
|
||||
if (calibration_counter % (calibration_maxcount / 20) == 0)
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
|
||||
|
||||
|
||||
} else {
|
||||
poll_errcount++;
|
||||
}
|
||||
|
||||
if (poll_errcount > 1000) {
|
||||
mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
|
||||
close(sub_mag);
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
|
||||
close(sub_mag);
|
||||
}
|
||||
|
||||
float sphere_x;
|
||||
|
@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd)
|
|||
float sphere_z;
|
||||
float sphere_radius;
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
|
||||
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
|
||||
if (res == OK) {
|
||||
/* sphere fit */
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
|
||||
sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
|
||||
|
||||
free(x);
|
||||
free(y);
|
||||
free(z);
|
||||
if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
|
||||
if (x != NULL)
|
||||
free(x);
|
||||
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
if (y != NULL)
|
||||
free(y);
|
||||
|
||||
if (z != NULL)
|
||||
free(z);
|
||||
|
||||
if (res == OK) {
|
||||
/* apply calibration and set parameters */
|
||||
struct mag_scale mscale;
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to get scale / offsets for mag");
|
||||
fd = open(MAG_DEVICE_PATH, 0);
|
||||
res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
|
||||
|
||||
mscale.x_offset = sphere_x;
|
||||
mscale.y_offset = sphere_y;
|
||||
mscale.z_offset = sphere_z;
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
|
||||
}
|
||||
|
||||
if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
|
||||
warn("WARNING: failed to set scale / offsets for mag");
|
||||
if (res == OK) {
|
||||
mscale.x_offset = sphere_x;
|
||||
mscale.y_offset = sphere_y;
|
||||
mscale.z_offset = sphere_z;
|
||||
|
||||
res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
close(fd);
|
||||
|
||||
/* announce and set new offset */
|
||||
if (res == OK) {
|
||||
/* set parameters */
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
|
||||
warnx("Setting X mag offset failed!\n");
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
|
||||
res = ERROR;
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
|
||||
res = ERROR;
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
|
||||
warnx("Setting Y mag offset failed!\n");
|
||||
if (res == OK) {
|
||||
/* auto-save to EEPROM */
|
||||
res = param_save_default();
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
}
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
|
||||
warnx("Setting Z mag offset failed!\n");
|
||||
mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
|
||||
(double)mscale.y_offset, (double)mscale.z_offset);
|
||||
mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
|
||||
(double)mscale.y_scale, (double)mscale.z_scale);
|
||||
|
||||
if (res == OK) {
|
||||
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
|
||||
warnx("Setting X mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
|
||||
warnx("Setting Y mag scale failed!\n");
|
||||
}
|
||||
|
||||
if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
|
||||
warnx("Setting Z mag scale failed!\n");
|
||||
}
|
||||
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
|
||||
|
||||
/* auto-save to EEPROM */
|
||||
int save_ret = param_save_default();
|
||||
|
||||
if (save_ret != 0) {
|
||||
warn("WARNING: auto-save of params to storage failed");
|
||||
mavlink_log_info(mavlink_fd, "FAILED storing calibration");
|
||||
close(sub_mag);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
|
||||
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
|
||||
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
|
||||
|
||||
char buf[52];
|
||||
sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
|
||||
(double)mscale.y_offset, (double)mscale.z_offset);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
|
||||
(double)mscale.y_scale, (double)mscale.z_scale);
|
||||
mavlink_log_info(mavlink_fd, buf);
|
||||
|
||||
mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
|
||||
mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
|
||||
|
||||
close(sub_mag);
|
||||
return OK;
|
||||
/* third beep by cal end routine */
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
|
||||
close(sub_mag);
|
||||
return ERROR;
|
||||
return res;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -47,4 +47,3 @@ SRCS = commander.cpp \
|
|||
baro_calibration.cpp \
|
||||
rc_calibration.cpp \
|
||||
airspeed_calibration.cpp
|
||||
|
||||
|
|
|
@ -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()
|
||||
{
|
||||
|
|
Loading…
Reference in New Issue