fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 17:23:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into sdlog2_ver

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-23 14:49:11 +02:00
parent 114b7b696d 17ddc7f471
commit 3546ded54e
63 changed files with 2908 additions and 760 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+211 -135
View File
@@ -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;
src/modules/commander/calibration_messages.h
+57
View File
@@ -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_ */
src/modules/commander/commander.cpp
+32 -26
View File
@@ -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:
@@ -687,7 +690,7 @@ int commander_thread_main(int argc, char *argv[])
bool updated = false;
bool rc_calibration_ok = (OK == rc_calibration_check());
bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
/* Subscribe to safety topic */
int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -802,7 +805,7 @@ int commander_thread_main(int argc, char *argv[])
status_changed = true;
/* re-check RC calibration */
rc_calibration_ok = (OK == rc_calibration_check());
rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
/* navigation parameters */
param_get(_param_takeoff_alt, &takeoff_alt);
@@ -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);
src/modules/commander/gyro_calibration.cpp
+149 -141
View File
@@ -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,21 +58,14 @@
#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");
const unsigned calibration_count = 5000;
int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
struct sensor_combined_s raw;
unsigned calibration_counter = 0;
float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
/* set offsets to zero */
int fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale_null = {
struct gyro_scale gyro_scale = {
0.0f,
1.0f,
0.0f,
@@ -79,98 +74,101 @@ int do_gyro_calibration(int mavlink_fd)
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
warn("WARNING: failed to set scale / offsets for gyro");
int res = OK;
/* reset all offsets to zero and all scales to one */
int fd = open(GYRO_DEVICE_PATH, 0);
res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
close(fd);
unsigned poll_errcount = 0;
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
}
while (calibration_counter < calibration_count) {
if (res == OK) {
/* determine gyro mean values */
const unsigned calibration_count = 5000;
unsigned calibration_counter = 0;
unsigned poll_errcount = 0;
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_combined;
fds[0].events = POLLIN;
/* subscribe to gyro sensor topic */
int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
struct gyro_report gyro_report;
int poll_ret = poll(fds, 1, 1000);
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
struct pollfd fds[1];
fds[0].fd = sub_sensor_gyro;
fds[0].events = POLLIN;
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++;
if (calibration_counter % (calibration_count / 20) == 0)
mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
int poll_ret = poll(fds, 1, 1000);
} else {
poll_errcount++;
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 (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
close(sub_sensor_combined);
return ERROR;
close(sub_sensor_gyro);
gyro_scale.x_offset /= calibration_count;
gyro_scale.y_offset /= calibration_count;
gyro_scale.z_offset /= calibration_count;
}
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;
}
}
gyro_offset[0] = gyro_offset[0] / calibration_count;
gyro_offset[1] = gyro_offset[1] / calibration_count;
gyro_offset[2] = gyro_offset[2] / calibration_count;
if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
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;
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
1.0f,
gyro_offset[1],
1.0f,
gyro_offset[2],
1.0f,
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets 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_combined);
return ERROR;
}
tune_neutral();
/* third beep by cal end routine */
} else {
mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
close(sub_sensor_combined);
return ERROR;
}
mavlink_log_info(mavlink_fd, "offset calibration done.");
#if 0
/*** --- SCALING --- ***/
/* beep on offset calibration end */
mavlink_log_info(mavlink_fd, "gyro offset calibration done");
tune_neutral();
/* 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.");
warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
/* apply new offsets */
fd = open(GYRO_DEVICE_PATH, 0);
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
warn("WARNING: failed to apply new offsets for gyro");
close(fd);
unsigned rotations_count = 30;
float gyro_integral = 0.0f;
float baseline_integral = 0.0f;
@@ -178,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();
@@ -190,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;
@@ -218,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;
@@ -235,63 +238,68 @@ 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);
#else
float gyro_scales[] = { 1.0f, 1.0f, 1.0f };
#endif
if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
}
/* set offsets to actual value */
fd = open(GYRO_DEVICE_PATH, 0);
struct gyro_scale gscale = {
gyro_offset[0],
gyro_scales[0],
gyro_offset[1],
gyro_scales[1],
gyro_offset[2],
gyro_scales[2],
};
if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
warn("WARNING: failed to set scale / offsets for gyro");
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, "gyro calibration done");
/* third beep by cal end routine */
close(sub_sensor_combined);
return OK;
} else {
mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
close(sub_sensor_combined);
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, "gyro scale calibration done");
tune_neutral();
#endif
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;
}
}
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 (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;
}
src/modules/commander/mag_calibration.cpp
+150 -152
View File
@@ -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;
}
}
src/modules/commander/module.mk
-1
View File
@@ -47,4 +47,3 @@ SRCS = commander.cpp \
baro_calibration.cpp \
rc_calibration.cpp \
airspeed_calibration.cpp
src/modules/sdlog2/sdlog2.c
+10 -1
View File
@@ -610,6 +610,15 @@ int sdlog2_thread_main(int argc, char *argv[])
errx(1, "unable to create logging folder, exiting.");
}
const char *converter_in = "/etc/logging/conv.zip";
char* converter_out = malloc(120);
sprintf(converter_out, "%s/conv.zip", folder_path);
if (file_copy(converter_in, converter_out)) {
errx(1, "unable to copy conversion scripts, exiting.");
}
free(converter_out);
/* only print logging path, important to find log file later */
warnx("logging to directory: %s", folder_path);
@@ -1278,7 +1287,7 @@ int file_copy(const char *file_old, const char *file_new)
fclose(source);
fclose(target);
return ret;
return OK;
}
void handle_command(struct vehicle_command_s *cmd)
src/modules/segway/BlockSegwayController.cpp
+7 -6
View File
@@ -26,7 +26,7 @@ void BlockSegwayController::update() {
_actuators.control[i] = 0.0f;
// only update guidance in auto mode
if (_status.state_machine == SYSTEM_STATE_AUTO) {
if (_status.main_state == MAIN_STATE_AUTO) {
// update guidance
}
@@ -34,17 +34,18 @@ void BlockSegwayController::update() {
float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
if (_status.main_state == MAIN_STATE_AUTO ||
_status.main_state == MAIN_STATE_SEATBELT ||
_status.main_state == MAIN_STATE_EASY) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
} else if (_status.main_state == MAIN_STATE_MANUAL) {
if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
_actuators.control[CH_LEFT] = _manual.throttle;
_actuators.control[CH_RIGHT] = _manual.pitch;
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
_actuators.control[0] = spdCmd;
_actuators.control[1] = spdCmd;
}
src/modules/sensors/sensor_params.c
+26
View File
@@ -44,8 +44,34 @@
#include <systemlib/param/param.h>
/**
* Gyro X offset FIXME
*
* This is an X-axis offset for the gyro.
* Adjust it according to the calibration data.
*
* @min -10.0
* @max 10.0
* @group Gyro Config
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
/**
* Gyro Y offset FIXME with dot.
*
* @min -10.0
* @max 10.0
* @group Gyro Config
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
/**
* Gyro Z offset FIXME
*
* @min -5.0
* @max 5.0
* @group Gyro Config
*/
PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
src/modules/sensors/sensors.cpp
+1 -92
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()
{
src/modules/systemlib/mixer/mixer.cpp
+29
View File
@@ -50,6 +50,8 @@
#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <ctype.h>
#include <systemlib/err.h>
#include "mixer.h"
@@ -114,6 +116,33 @@ Mixer::scale_check(struct mixer_scaler_s &scaler)
return 0;
}
const char *
Mixer::findtag(const char *buf, unsigned &buflen, char tag)
{
while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':'))
return buf;
buf++;
buflen--;
}
return nullptr;
}
const char *
Mixer::skipline(const char *buf, unsigned &buflen)
{
const char *p;
/* if we can find a CR or NL in the buffer, skip up to it */
if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen))) {
/* skip up to it AND one beyond - could be on the NUL symbol now */
buflen -= (p - buf) + 1;
return p + 1;
}
return nullptr;
}
/****************************************************************************/
NullMixer::NullMixer() :
src/modules/systemlib/mixer/mixer.h
+25
View File
@@ -128,7 +128,9 @@
#ifndef _SYSTEMLIB_MIXER_MIXER_H
#define _SYSTEMLIB_MIXER_MIXER_H value
#include <nuttx/config.h>
#include "drivers/drv_mixer.h"
#include "mixer_load.h"
/**
* Abstract class defining a mixer mixing zero or more inputs to
@@ -210,6 +212,24 @@ protected:
*/
static int scale_check(struct mixer_scaler_s &scaler);
/**
* Find a tag
*
* @param buf The buffer to operate on.
* @param buflen length of the buffer.
* @param tag character to search for.
*/
static const char * findtag(const char *buf, unsigned &buflen, char tag);
/**
* Skip a line
*
* @param buf The buffer to operate on.
* @param buflen length of the buffer.
* @return 0 / OK if a line could be skipped, 1 else
*/
static const char * skipline(const char *buf, unsigned &buflen);
private:
};
@@ -238,6 +258,11 @@ public:
*/
void reset();
/**
* Count the mixers in the group.
*/
unsigned count();
/**
* Adds mixers to the group based on a text description in a buffer.
*
src/modules/systemlib/mixer/mixer_group.cpp
+15
View File
@@ -111,6 +111,20 @@ MixerGroup::mix(float *outputs, unsigned space)
return index;
}
unsigned
MixerGroup::count()
{
Mixer *mixer = _first;
unsigned index = 0;
while ((mixer != nullptr)) {
mixer = mixer->_next;
index++;
}
return index;
}
void
MixerGroup::groups_required(uint32_t &groups)
{
@@ -170,6 +184,7 @@ MixerGroup::load_from_buf(const char *buf, unsigned &buflen)
/* only adjust buflen if parsing was successful */
buflen = resid;
debug("SUCCESS - buflen: %d", buflen);
} else {
/*
src/modules/systemlib/mixer/mixer_load.c
+100
View File
@@ -0,0 +1,100 @@
/****************************************************************************
*
* Copyright (C) 2012 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 mixer_load.c
*
* Programmable multi-channel mixer library.
*/
#include <nuttx/config.h>
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include "mixer_load.h"
int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
{
FILE *fp;
char line[120];
/* open the mixer definition file */
fp = fopen(fname, "r");
if (fp == NULL) {
return 1;
}
/* read valid lines from the file into a buffer */
buf[0] = '\0';
for (;;) {
/* get a line, bail on error/EOF */
line[0] = '\0';
if (fgets(line, sizeof(line), fp) == NULL)
break;
/* if the line doesn't look like a mixer definition line, skip it */
if ((strlen(line) < 2) || !isupper(line[0]) || (line[1] != ':'))
continue;
/* compact whitespace in the buffer */
char *t, *f;
for (f = line; *f != '\0'; f++) {
/* scan for space characters */
if (*f == ' ') {
/* look for additional spaces */
t = f + 1;
while (*t == ' ')
t++;
if (*t == '\0') {
/* strip trailing whitespace */
*f = '\0';
} else if (t > (f + 1)) {
memmove(f + 1, t, strlen(t) + 1);
}
}
}
/* if the line is too long to fit in the buffer, bail */
if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
return 1;
}
/* add the line to the buffer */
strcat(buf, line);
}
return 0;
}
src/modules/systemlib/mixer/mixer_load.h
+51
View File
@@ -0,0 +1,51 @@
/****************************************************************************
*
* Copyright (C) 2012 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 mixer_load.h
*
*/
#ifndef _SYSTEMLIB_MIXER_LOAD_H
#define _SYSTEMLIB_MIXER_LOAD_H value
#include <nuttx/config.h>
__BEGIN_DECLS
__EXPORT int load_mixer_file(const char *fname, char *buf, unsigned maxlen);
__END_DECLS
#endif
src/modules/systemlib/mixer/mixer_multirotor.cpp
+10 -4
View File
@@ -130,7 +130,7 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
{ 1.000000, 0.000000, -1.00 },
{ -1.000000, 0.000000, -1.00 },
};
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
&_config_quad_x[0],
&_config_quad_plus[0],
&_config_quad_v[0],
@@ -140,7 +140,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::Geometry::MAX_GEOME
&_config_octa_x[0],
&_config_octa_plus[0],
};
const unsigned _config_rotor_count[MultirotorMixer::Geometry::MAX_GEOMETRY] = {
const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
4, /* quad_x */
4, /* quad_plus */
4, /* quad_v */
@@ -205,11 +205,17 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
}
if (used > (int)buflen) {
debug("multirotor spec used %d of %u", used, buflen);
debug("OVERFLOW: multirotor spec used %d of %u", used, buflen);
return nullptr;
}
buflen -= used;
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return nullptr;
}
debug("remaining in buf: %d, first char: %c", buflen, buf[0]);
if (!strcmp(geomname, "4+")) {
geometry = MultirotorMixer::QUAD_PLUS;
src/modules/systemlib/mixer/mixer_simple.cpp
+19 -43
View File
@@ -55,7 +55,7 @@
#include "mixer.h"
#define debug(fmt, args...) do { } while(0)
// #define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
//#define debug(fmt, args...) do { printf("[mixer] " fmt "\n", ##args); } while(0)
SimpleMixer::SimpleMixer(ControlCallback control_cb,
uintptr_t cb_handle,
@@ -71,28 +71,6 @@ SimpleMixer::~SimpleMixer()
free(_info);
}
static const char *
findtag(const char *buf, unsigned &buflen, char tag)
{
while (buflen >= 2) {
if ((buf[0] == tag) && (buf[1] == ':'))
return buf;
buf++;
buflen--;
}
return nullptr;
}
static void
skipline(const char *buf, unsigned &buflen)
{
const char *p;
/* if we can find a CR or NL in the buffer, skip up to it */
if ((p = (const char *)memchr(buf, '\r', buflen)) || (p = (const char *)memchr(buf, '\n', buflen)))
buflen -= (p - buf);
}
int
SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler_s &scaler)
{
@@ -111,7 +89,12 @@ SimpleMixer::parse_output_scaler(const char *buf, unsigned &buflen, mixer_scaler
debug("out scaler parse failed on '%s' (got %d, consumed %d)", buf, ret, n);
return -1;
}
skipline(buf, buflen);
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return -1;
}
scaler.negative_scale = s[0] / 10000.0f;
scaler.positive_scale = s[1] / 10000.0f;
@@ -130,7 +113,7 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
buf = findtag(buf, buflen, 'S');
if ((buf == nullptr) || (buflen < 16)) {
debug("contorl parser failed finding tag, ret: '%s'", buf);
debug("control parser failed finding tag, ret: '%s'", buf);
return -1;
}
@@ -139,7 +122,12 @@ SimpleMixer::parse_control_scaler(const char *buf, unsigned &buflen, mixer_scale
debug("control parse failed on '%s'", buf);
return -1;
}
skipline(buf, buflen);
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
return -1;
}
control_group = u[0];
control_index = u[1];
@@ -161,29 +149,17 @@ SimpleMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handle, c
int used;
const char *end = buf + buflen;
/* enforce that the mixer ends with space or a new line */
for (int i = buflen - 1; i >= 0; i--) {
if (buf[i] == '\0')
continue;
/* require a space or newline at the end of the buffer, fail on printable chars */
if (buf[i] == ' ' || buf[i] == '\n' || buf[i] == '\r') {
/* found a line ending or space, so no split symbols / numbers. good. */
break;
} else {
debug("simple parser rejected: No newline / space at end of buf. (#%d/%d: 0x%02x)", i, buflen-1, buf[i]);
goto out;
}
}
/* get the base info for the mixer */
if (sscanf(buf, "M: %u%n", &inputs, &used) != 1) {
debug("simple parse failed on '%s'", buf);
goto out;
}
buflen -= used;
buf = skipline(buf, buflen);
if (buf == nullptr) {
debug("no line ending, line is incomplete");
goto out;
}
mixinfo = (mixer_simple_s *)malloc(MIXER_SIMPLE_SIZE(inputs));
src/modules/systemlib/mixer/module.mk
+2 -1
View File
@@ -39,4 +39,5 @@ LIBNAME = mixerlib
SRCS = mixer.cpp \
mixer_group.cpp \
mixer_multirotor.cpp \
mixer_simple.cpp
mixer_simple.cpp \
mixer_load.c
src/modules/systemlib/param/param.c
+47 -48
View File
@@ -508,64 +508,63 @@ param_get_default_file(void)
int
param_save_default(void)
{
int result;
unsigned retries = 0;
/* delete the file in case it exists */
struct stat buffer;
if (stat(param_get_default_file(), &buffer) == 0) {
do {
result = unlink(param_get_default_file());
if (result != 0) {
retries++;
usleep(1000 * retries);
}
} while (result != OK && retries < 10);
if (result != OK)
warnx("unlinking file %s failed.", param_get_default_file());
}
/* create the file */
int res;
int fd;
do {
/* do another attempt in case the unlink call is not synced yet */
fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
const char *filename = param_get_default_file();
const char *filename_tmp = malloc(strlen(filename) + 5);
sprintf(filename_tmp, "%s.tmp", filename);
/* delete temp file if exist */
res = unlink(filename_tmp);
if (res != OK && errno == ENOENT)
res = OK;
if (res != OK)
warn("failed to delete temp file: %s", filename_tmp);
if (res == OK) {
/* write parameters to temp file */
fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
if (fd < 0) {
retries++;
usleep(1000 * retries);
warn("failed to open temp file: %s", filename_tmp);
res = ERROR;
}
} while (fd < 0 && retries < 10);
if (res == OK) {
res = param_export(fd, false);
if (fd < 0) {
warn("opening '%s' for writing failed", param_get_default_file());
return fd;
}
do {
result = param_export(fd, false);
if (result != OK) {
retries++;
usleep(1000 * retries);
if (res != OK)
warnx("failed to write parameters to file: %s", filename_tmp);
}
} while (result != 0 && retries < 10);
close(fd);
if (result != OK) {
warn("error exporting parameters to '%s'", param_get_default_file());
(void)unlink(param_get_default_file());
return result;
close(fd);
}
return 0;
if (res == OK) {
/* delete parameters file */
res = unlink(filename);
if (res != OK && errno == ENOENT)
res = OK;
if (res != OK)
warn("failed to delete parameters file: %s", filename);
}
if (res == OK) {
/* rename temp file to parameters */
res = rename(filename_tmp, filename);
if (res != OK)
warn("failed to rename %s to %s", filename_tmp, filename);
}
free(filename_tmp);
return res;
}
/**
src/modules/systemlib/rc_check.c
+1 -3
View File
@@ -47,14 +47,12 @@
#include <mavlink/mavlink_log.h>
#include <uORB/topics/rc_channels.h>
int rc_calibration_check(void) {
int rc_calibration_check(int mavlink_fd) {
char nbuf[20];
param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
_parameter_handles_rev, _parameter_handles_dz;
int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
float param_min, param_max, param_trim, param_rev, param_dz;
/* first check channel mappings */
src/modules/systemlib/rc_check.h
+1 -1
View File
@@ -47,6 +47,6 @@
* @return 0 / OK if RC calibration is ok, index + 1 of the first
* channel that failed else (so 1 == first channel failed)
*/
__EXPORT int rc_calibration_check(void);
__EXPORT int rc_calibration_check(int mavlink_fd);
__END_DECLS