fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-21 14:27:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' into inav_alt_gps. Use GPS for altitude estimation.

Browse Source
This commit is contained in:
Anton Babushkin
2013-10-27 22:03:07 +04:00
parent 7d4981d4b4 59c04adada
commit d309d0203c
73 changed files with 3361 additions and 874 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);
@@ -941,7 +944,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);
}
@@ -971,6 +974,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);
}
@@ -1245,12 +1249,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);
@@ -1265,7 +1271,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);
@@ -1309,6 +1315,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);
@@ -1333,6 +1340,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 {
@@ -1706,11 +1714,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;
@@ -1785,7 +1792,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) {
@@ -1866,7 +1873,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/fw_att_control/fw_att_control_main.cpp
+4
View File
@@ -683,6 +683,10 @@ FixedwingAttitudeControl::task_main()
_actuators.control[4] = _manual.flaps;
}
_actuators.control[5] = _manual.aux1;
_actuators.control[6] = _manual.aux2;
_actuators.control[7] = _manual.aux3;
/* lazily publish the setpoint only once available */
_actuators.timestamp = hrt_absolute_time();
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+157 -17
View File
@@ -158,6 +158,12 @@ private:
float _launch_alt;
bool _launch_valid;
/* land states */
/* not in non-abort mode for landing yet */
bool land_noreturn;
/* heading hold */
float target_bearing;
/* throttle and airspeed states */
float _airspeed_error; ///< airspeed error to setpoint in m/s
bool _airspeed_valid; ///< flag if a valid airspeed estimate exists
@@ -192,10 +198,13 @@ private:
float pitch_limit_min;
float pitch_limit_max;
float roll_limit;
float throttle_min;
float throttle_max;
float throttle_cruise;
float throttle_land_max;
float loiter_hold_radius;
} _parameters; /**< local copies of interesting parameters */
@@ -223,10 +232,13 @@ private:
param_t pitch_limit_min;
param_t pitch_limit_max;
param_t roll_limit;
param_t throttle_min;
param_t throttle_max;
param_t throttle_cruise;
param_t throttle_land_max;
param_t loiter_hold_radius;
} _parameter_handles; /**< handles for interesting parameters */
@@ -325,7 +337,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
_airspeed_error(0.0f),
_airspeed_valid(false),
_groundspeed_undershoot(0.0f),
_global_pos_valid(false)
_global_pos_valid(false),
land_noreturn(false)
{
_nav_capabilities.turn_distance = 0.0f;
@@ -339,9 +352,11 @@ FixedwingPositionControl::FixedwingPositionControl() :
_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
_parameter_handles.roll_limit = param_find("FW_R_LIM");
_parameter_handles.throttle_min = param_find("FW_THR_MIN");
_parameter_handles.throttle_max = param_find("FW_THR_MAX");
_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
_parameter_handles.time_const = param_find("FW_T_TIME_CONST");
_parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN");
@@ -400,10 +415,13 @@ FixedwingPositionControl::parameters_update()
param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
param_get(_parameter_handles.time_const, &(_parameters.time_const));
param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
@@ -419,6 +437,7 @@ FixedwingPositionControl::parameters_update()
_l1_control.set_l1_damping(_parameters.l1_damping);
_l1_control.set_l1_period(_parameters.l1_period);
_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
_tecs.set_time_const(_parameters.time_const);
_tecs.set_min_sink_rate(_parameters.min_sink_rate);
@@ -625,6 +644,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
/* no throttle limit as default */
float throttle_max = 1.0f;
/* AUTONOMOUS FLIGHT */
// XXX this should only execute if auto AND safety off (actuators active),
@@ -634,11 +656,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
/* get circle mode */
bool was_circle_mode = _l1_control.circle_mode();
/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
_tecs.set_speed_weight(_parameters.speed_weight);
/* execute navigation once we have a setpoint */
if (_setpoint_valid) {
float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
/* current waypoint (the one currently heading for) */
math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
@@ -706,27 +729,87 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
/* switch to heading hold for the last meters, continue heading hold after */
float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
if (wp_distance < 15.0f || land_noreturn) {
/* heading hold, along the line connecting this and the last waypoint */
// if (global_triplet.previous_valid) {
// target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
// } else {
if (!land_noreturn)
target_bearing = _att.yaw;
//}
warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
if (altitude_error > -5.0f)
land_noreturn = true;
} else {
/* normal navigation */
_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
}
/* do not go down too early */
if (wp_distance > 50.0f) {
altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
}
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
// XXX this could make a great param
if (altitude_error > -20.0f) {
float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1)
float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
float land_pitch_min = math::radians(5.0f);
float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
float airspeed_land = _parameters.airspeed_min;
float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
if (altitude_error > -4.0f) {
/* land with minimal speed */
/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
_tecs.set_speed_weight(2.0f);
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
_airspeed.indicated_airspeed_m_s, eas2tas,
true, flare_angle_rad,
false, flare_angle_rad,
0.0f, _parameters.throttle_max, throttle_land,
math::radians(-10.0f), math::radians(15.0f));
/* kill the throttle if param requests it */
throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
/* minimize speed to approach speed */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, flare_angle_rad,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
/* limit roll motion to prevent wings from touching the ground first */
_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
} else {
/* normal cruise speed */
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
_airspeed.indicated_airspeed_m_s, eas2tas,
false, math::radians(_parameters.pitch_limit_min),
@@ -785,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_loiter_hold = true;
}
float altitude_error = _loiter_hold_alt - _global_pos.alt;
altitude_error = _loiter_hold_alt - _global_pos.alt;
math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
@@ -796,7 +879,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
_att_sp.yaw_body = _l1_control.nav_bearing();
/* climb with full throttle if the altitude error is bigger than 5 meters */
bool climb_out = (altitude_error > 5);
bool climb_out = (altitude_error > 3);
float min_pitch;
@@ -819,11 +902,53 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
}
/* reset land state */
if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
land_noreturn = false;
}
if (was_circle_mode && !_l1_control.circle_mode()) {
/* just kicked out of loiter, reset roll integrals */
_att_sp.roll_reset_integral = true;
}
} else if (0/* easy mode enabled */) {
/** EASY FLIGHT **/
if (0/* switched from another mode to easy */) {
_seatbelt_hold_heading = _att.yaw;
}
if (0/* easy on and manual control yaw non-zero */) {
_seatbelt_hold_heading = _att.yaw + _manual.yaw;
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
/* if in seatbelt mode, set airspeed based on manual control */
// XXX check if ground speed undershoot should be applied here
float seatbelt_airspeed = _parameters.airspeed_min +
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
} else if (0/* seatbelt mode enabled */) {
/** SEATBELT FLIGHT **/
@@ -843,13 +968,28 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
(_parameters.airspeed_max - _parameters.airspeed_min) *
_manual.throttle;
/* user switched off throttle */
if (_manual.throttle < 0.1f) {
throttle_max = 0.0f;
/* switch to pure pitch based altitude control, give up speed */
_tecs.set_speed_weight(0.0f);
}
/* climb out control */
bool climb_out = false;
/* user wants to climb out */
if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
climb_out = true;
}
_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
_att_sp.roll_body = _l1_control.nav_roll();
_att_sp.yaw_body = _l1_control.nav_bearing();
_att_sp.roll_body = _manual.roll;
_att_sp.yaw_body = _manual.yaw;
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
seatbelt_airspeed,
_airspeed.indicated_airspeed_m_s, eas2tas,
false, _parameters.pitch_limit_min,
climb_out, _parameters.pitch_limit_min,
_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
_parameters.pitch_limit_min, _parameters.pitch_limit_max);
@@ -862,7 +1002,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
}
_att_sp.pitch_body = _tecs.get_pitch_demand();
_att_sp.thrust = _tecs.get_throttle_demand();
_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
return setpoint;
}
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+4
View File
@@ -67,11 +67,15 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
src/modules/mavlink/mavlink_receiver.cpp
+2 -2
View File
@@ -422,13 +422,13 @@ handle_message(mavlink_message_t *msg)
hil_sensors.accelerometer_counter = hil_counter;
/* differential pressure */
hil_sensors.differential_pressure_pa = imu.diff_pressure;
hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
hil_sensors.differential_pressure_counter = hil_counter;
/* airspeed from differential pressure, ambient pressure and temp */
struct airspeed_s airspeed;
float ias = calc_indicated_airspeed(imu.diff_pressure);
float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
// XXX need to fix this
float tas = ias;
src/modules/mavlink/waypoints.c
+25 -12
View File
@@ -398,7 +398,14 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
if (time_elapsed) {
/* safeguard against invalid missions with last wp autocontinue on */
if (wpm->current_active_wp_id == wpm->size - 1) {
/* stop handling missions here */
cur_wp->autocontinue = false;
}
if (cur_wp->autocontinue) {
cur_wp->current = 0;
float navigation_lat = -1.0f;
@@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
navigation_frame = MAV_FRAME_LOCAL_NED;
}
/* guard against missions without final land waypoint */
/* only accept supported navigation waypoints, skip unknown ones */
do {
/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
/* this is a navigation waypoint */
navigation_frame = cur_wp->frame;
@@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
navigation_alt = cur_wp->z;
}
if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
/* the last waypoint was reached, if auto continue is
* activated keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
cur_wp->autocontinue = false;
if (wpm->current_active_wp_id == wpm->size - 1) {
/* if we're not landing at the last nav waypoint, we're falling back to loiter */
if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
/* the last waypoint was reached, if auto continue is
* activated AND it is NOT a land waypoint, keep the system loitering there.
*/
cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
cur_wp->frame = navigation_frame;
cur_wp->x = navigation_lat;
cur_wp->y = navigation_lon;
cur_wp->z = navigation_alt;
}
/* we risk an endless loop for missions without navigation waypoints, abort. */
break;
src/modules/multirotor_att_control/multirotor_att_control_main.c
+6
View File
@@ -368,6 +368,12 @@ mc_thread_main(int argc, char *argv[])
actuators.control[3] = 0.0f;
}
/* fill in manual control values */
actuators.control[4] = manual.flaps;
actuators.control[5] = manual.aux1;
actuators.control[6] = manual.aux2;
actuators.control[7] = manual.aux3;
actuators.timestamp = hrt_absolute_time();
orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
src/modules/position_estimator_inav/position_estimator_inav_main.c
+119 -92
View File
@@ -71,6 +71,8 @@
#include "position_estimator_inav_params.h"
#include "inertial_filter.h"
#define MIN_VALID_W 0.00001f
static bool thread_should_exit = false; /**< Deamon exit flag */
static bool thread_running = false; /**< Deamon status flag */
static int position_estimator_inav_task; /**< Handle of deamon task / thread */
@@ -180,7 +182,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
int baro_init_cnt = 0;
int baro_init_num = 200;
float baro_offset = 0.0f; // baro offset for reference altitude, initialized only once
float baro_offset = 0.0f; // baro offset for reference altitude, initialized on start, then adjusted
float surface_offset = 0.0f; // ground level offset from reference altitude
float surface_offset_rate = 0.0f; // surface offset change rate
float alt_avg = 0.0f;
@@ -192,9 +194,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
uint32_t accel_counter = 0;
uint32_t baro_counter = 0;
bool ref_xy_inited = false;
hrt_abstime ref_xy_init_start = 0;
const hrt_abstime ref_xy_init_delay = 1000000; // wait for 1s after 3D fix
bool ref_inited = false;
hrt_abstime ref_init_start = 0;
const hrt_abstime ref_init_delay = 1000000; // wait for 1s after 3D fix
uint16_t accel_updates = 0;
uint16_t baro_updates = 0;
@@ -211,18 +213,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
float accel_corr[] = { 0.0f, 0.0f, 0.0f }; // N E D
float accel_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float baro_corr = 0.0f; // D
float gps_corr[2][2] = {
float corr_acc[] = { 0.0f, 0.0f, 0.0f }; // N E D
float acc_bias[] = { 0.0f, 0.0f, 0.0f }; // body frame
float corr_baro = 0.0f; // D
float corr_gps[3][2] = {
{ 0.0f, 0.0f }, // N (pos, vel)
{ 0.0f, 0.0f }, // E (pos, vel)
{ 0.0f, 0.0f }, // D (pos, vel)
};
float sonar_corr = 0.0f;
float sonar_corr_filtered = 0.0f;
float w_gps_xy = 1.0f;
float w_gps_z = 1.0f;
float corr_sonar = 0.0f;
float corr_sonar_filtered = 0.0f;
float flow_corr[] = { 0.0f, 0.0f }; // X, Y
float flow_w = 0.0f;
float corr_flow[] = { 0.0f, 0.0f }; // N E
float w_flow = 0.0f;
float sonar_prev = 0.0f;
hrt_abstime sonar_time = 0; // time of last sonar measurement (not filtered)
@@ -307,13 +312,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else {
wait_baro = false;
baro_offset /= (float) baro_init_cnt;
warnx("init ref: alt=%.3f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] init ref: alt=%.3f", baro_offset);
local_pos.ref_alt = baro_offset;
local_pos.ref_timestamp = hrt_absolute_time();
corr_baro = -baro_offset;
warnx("baro offs: %.2f", baro_offset);
mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", baro_offset);
local_pos.z_valid = true;
local_pos.v_z_valid = true;
local_pos.z_global = true;
}
}
}
@@ -370,10 +373,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (armed.armed && !flag_armed) {
flag_armed = armed.armed;
baro_offset -= z_est[0];
corr_baro = -baro_offset;
local_pos.ref_alt -= z_est[0];
local_pos.ref_timestamp = t;
z_est[0] = 0.0f;
alt_avg = 0.0f;
local_pos.ref_alt = baro_offset;
local_pos.ref_timestamp = t;
}
}
@@ -386,9 +390,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (sensor.accelerometer_counter > accel_counter) {
if (att.R_valid) {
/* correct accel bias, now only for Z */
sensor.accelerometer_m_s2[0] -= accel_bias[0];
sensor.accelerometer_m_s2[1] -= accel_bias[1];
sensor.accelerometer_m_s2[2] -= accel_bias[2];
sensor.accelerometer_m_s2[0] -= acc_bias[0];
sensor.accelerometer_m_s2[1] -= acc_bias[1];
sensor.accelerometer_m_s2[2] -= acc_bias[2];
/* transform acceleration vector from body frame to NED frame */
for (int i = 0; i < 3; i++) {
@@ -399,12 +403,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
}
accel_corr[0] = accel_NED[0] - x_est[2];
accel_corr[1] = accel_NED[1] - y_est[2];
accel_corr[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
corr_acc[0] = accel_NED[0] - x_est[2];
corr_acc[1] = accel_NED[1] - y_est[2];
corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
} else {
memset(accel_corr, 0, sizeof(accel_corr));
memset(corr_acc, 0, sizeof(corr_acc));
}
accel_counter = sensor.accelerometer_counter;
@@ -412,7 +416,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
if (sensor.baro_counter > baro_counter) {
baro_corr = baro_offset - sensor.baro_alt_meter - z_est[0];
corr_baro = - sensor.baro_alt_meter - z_est[0];
baro_counter = sensor.baro_counter;
baro_updates++;
}
@@ -427,22 +431,22 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (flow.ground_distance_m > 0.31f && flow.ground_distance_m < 4.0f && att.R[2][2] > 0.7 && flow.ground_distance_m != sonar_prev) {
sonar_time = t;
sonar_prev = flow.ground_distance_m;
sonar_corr = flow.ground_distance_m + surface_offset + z_est[0];
sonar_corr_filtered += (sonar_corr - sonar_corr_filtered) * params.sonar_filt;
corr_sonar = flow.ground_distance_m + surface_offset + z_est[0];
corr_sonar_filtered += (corr_sonar - corr_sonar_filtered) * params.sonar_filt;
if (fabsf(sonar_corr) > params.sonar_err) {
if (fabsf(corr_sonar) > params.sonar_err) {
/* correction is too large: spike or new ground level? */
if (fabsf(sonar_corr - sonar_corr_filtered) > params.sonar_err) {
if (fabsf(corr_sonar - corr_sonar_filtered) > params.sonar_err) {
/* spike detected, ignore */
sonar_corr = 0.0f;
corr_sonar = 0.0f;
sonar_valid = false;
} else {
/* new ground level */
surface_offset -= sonar_corr;
surface_offset -= corr_sonar;
surface_offset_rate = 0.0f;
sonar_corr = 0.0f;
sonar_corr_filtered = 0.0f;
corr_sonar = 0.0f;
corr_sonar_filtered = 0.0f;
sonar_valid_time = t;
sonar_valid = true;
local_pos.surface_bottom_timestamp = t;
@@ -494,21 +498,21 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* velocity correction */
flow_corr[0] = flow_v[0] - x_est[1];
flow_corr[1] = flow_v[1] - y_est[1];
corr_flow[0] = flow_v[0] - x_est[1];
corr_flow[1] = flow_v[1] - y_est[1];
/* adjust correction weight */
float flow_q_weight = (flow_q - params.flow_q_min) / (1.0f - params.flow_q_min);
flow_w = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
w_flow = att.R[2][2] * flow_q_weight / fmaxf(1.0f, flow_dist);
/* if flow is not accurate, reduce weight for it */
// TODO make this more fuzzy
if (!flow_accurate)
flow_w *= 0.05f;
w_flow *= 0.05f;
flow_valid = true;
} else {
flow_w = 0.0f;
w_flow = 0.0f;
flow_valid = false;
}
@@ -524,13 +528,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps.fix_type >= 3) {
/* hysteresis for GPS quality */
if (gps_valid) {
if (gps.eph_m > 10.0f) {
if (gps.eph_m > 10.0f || gps.epv_m > 10.0f) {
gps_valid = false;
mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
}
} else {
if (gps.eph_m < 5.0f) {
if (gps.eph_m < 5.0f && gps.epv_m < 5.0f) {
gps_valid = true;
mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
}
@@ -542,50 +546,58 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (gps_valid) {
/* initialize reference position if needed */
if (!ref_xy_inited) {
if (ref_xy_init_start == 0) {
ref_xy_init_start = t;
if (!ref_inited) {
if (ref_init_start == 0) {
ref_init_start = t;
} else if (t > ref_xy_init_start + ref_xy_init_delay) {
ref_xy_inited = true;
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* reference GPS position */
double lat = gps.lat * 1e-7;
double lon = gps.lon * 1e-7;
float alt = gps.alt * 1e-3;
local_pos.ref_lat = gps.lat;
local_pos.ref_lon = gps.lon;
local_pos.ref_alt = alt + z_est[0];
local_pos.ref_timestamp = t;
/* initialize projection */
map_projection_init(lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f", lat, lon);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f", lat, lon);
warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
mavlink_log_info(mavlink_fd, "[inav] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt);
}
}
if (ref_xy_inited) {
if (ref_inited) {
/* project GPS lat lon to plane */
float gps_proj[2];
map_projection_project(gps.lat * 1e-7, gps.lon * 1e-7, &(gps_proj[0]), &(gps_proj[1]));
/* calculate correction for position */
gps_corr[0][0] = gps_proj[0] - x_est[0];
gps_corr[1][0] = gps_proj[1] - y_est[0];
corr_gps[0][0] = gps_proj[0] - x_est[0];
corr_gps[1][0] = gps_proj[1] - y_est[0];
corr_gps[2][0] = local_pos.ref_alt - gps.alt * 1e-3 - z_est[0];
/* calculate correction for velocity */
if (gps.vel_ned_valid) {
gps_corr[0][1] = gps.vel_n_m_s - x_est[1];
gps_corr[1][1] = gps.vel_e_m_s - y_est[1];
corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
} else {
gps_corr[0][1] = 0.0f;
gps_corr[1][1] = 0.0f;
corr_gps[0][1] = 0.0f;
corr_gps[1][1] = 0.0f;
corr_gps[2][1] = 0.0f;
}
w_gps_xy = 1.0f / fmaxf(1.0f, gps.eph_m);
w_gps_z = 1.0f / fmaxf(1.0f, gps.epv_m);
}
} else {
/* no GPS lock */
memset(gps_corr, 0, sizeof(gps_corr));
ref_xy_init_start = 0;
memset(corr_gps, 0, sizeof(corr_gps));
ref_init_start = 0;
}
gps_updates++;
@@ -609,7 +621,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* check for sonar measurement timeout */
if (sonar_valid && t > sonar_time + sonar_timeout) {
sonar_corr = 0.0f;
corr_sonar = 0.0f;
sonar_valid = false;
}
@@ -617,12 +629,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
t_prev = t;
/* use GPS if it's valid and reference position initialized */
bool use_gps = ref_xy_inited && gps_valid && params.w_pos_gps_p > 0.00001f;
bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps);
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
/* try to estimate position during some time after position sources lost */
if (use_gps || use_flow) {
if (use_gps_xy || use_flow) {
xy_src_time = t;
}
@@ -636,36 +649,47 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* surface distance correction */
if (sonar_valid) {
surface_offset_rate -= sonar_corr * 0.5f * params.w_alt_sonar * params.w_alt_sonar * dt;
surface_offset -= sonar_corr * params.w_alt_sonar * dt;
surface_offset_rate -= corr_sonar * 0.5f * params.w_z_sonar * params.w_z_sonar * dt;
surface_offset -= corr_sonar * params.w_z_sonar * dt;
}
}
/* reduce GPS weight if optical flow is good */
float w_pos_gps_p = params.w_pos_gps_p;
float w_pos_gps_v = params.w_pos_gps_v;
float w_xy_gps_p = params.w_xy_gps_p * w_gps_xy;
float w_xy_gps_v = params.w_xy_gps_v * w_gps_xy;
float w_z_gps_p = params.w_z_gps_p * w_gps_z;
float w_z_gps_v = params.w_z_gps_v * w_gps_z;
/* reduce GPS weight if optical flow is good */
if (use_flow && flow_accurate) {
w_pos_gps_p *= params.w_gps_flow;
w_pos_gps_v *= params.w_gps_flow;
w_xy_gps_p *= params.w_gps_flow;
w_xy_gps_v *= params.w_gps_flow;
}
/* baro offset correction */
if (use_gps_z) {
baro_offset += corr_gps[2][0] * w_z_gps_p * dt;
}
/* accelerometer bias correction */
float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
if (use_gps) {
accel_bias_corr[0] -= gps_corr[0][0] * w_pos_gps_p * w_pos_gps_p;
accel_bias_corr[0] -= gps_corr[0][1] * w_pos_gps_v;
accel_bias_corr[1] -= gps_corr[1][0] * w_pos_gps_p * w_pos_gps_p;
accel_bias_corr[1] -= gps_corr[1][1] * w_pos_gps_v;
if (use_gps_xy) {
accel_bias_corr[0] -= corr_gps[0][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[0] -= corr_gps[0][1] * w_xy_gps_v;
accel_bias_corr[1] -= corr_gps[1][0] * w_xy_gps_p * w_xy_gps_p;
accel_bias_corr[1] -= corr_gps[1][1] * w_xy_gps_v;
}
if (use_gps_z) {
accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
accel_bias_corr[2] -= corr_gps[2][1] * w_z_gps_v;
}
if (use_flow) {
accel_bias_corr[0] -= flow_corr[0] * params.w_pos_flow;
accel_bias_corr[1] -= flow_corr[1] * params.w_pos_flow;
accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
}
accel_bias_corr[2] -= baro_corr * params.w_alt_baro * params.w_alt_baro;
accel_bias_corr[2] -= (corr_baro + baro_offset) * params.w_z_baro * params.w_z_baro;
/* transform error vector from NED frame to body frame */
for (int i = 0; i < 3; i++) {
@@ -675,15 +699,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
c += att.R[j][i] * accel_bias_corr[j];
}
accel_bias[i] += c * params.w_acc_bias * dt;
acc_bias[i] += c * params.w_acc_bias * dt;
}
/* inertial filter prediction for altitude */
inertial_filter_predict(dt, z_est);
/* inertial filter correction for altitude */
inertial_filter_correct(baro_corr, dt, z_est, 0, params.w_alt_baro);
inertial_filter_correct(accel_corr[2], dt, z_est, 2, params.w_alt_acc);
inertial_filter_correct(corr_baro + baro_offset, dt, z_est, 0, params.w_z_baro);
inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
inertial_filter_correct(corr_gps[2][1], dt, z_est, 1, w_z_gps_v);
inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
if (can_estimate_xy) {
/* inertial filter prediction for position */
@@ -696,27 +722,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
}
/* inertial filter correction for position */
inertial_filter_correct(accel_corr[0], dt, x_est, 2, params.w_pos_acc);
inertial_filter_correct(accel_corr[1], dt, y_est, 2, params.w_pos_acc);
inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
if (use_flow) {
inertial_filter_correct(flow_corr[0], dt, x_est, 1, params.w_pos_flow * flow_w);
inertial_filter_correct(flow_corr[1], dt, y_est, 1, params.w_pos_flow * flow_w);
inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
}
if (use_gps) {
inertial_filter_correct(gps_corr[0][0], dt, x_est, 0, w_pos_gps_p);
inertial_filter_correct(gps_corr[1][0], dt, y_est, 0, w_pos_gps_p);
if (use_gps_xy) {
inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_v);
if (gps.vel_ned_valid && t < gps.timestamp_velocity + gps_topic_timeout) {
inertial_filter_correct(gps_corr[0][1], dt, x_est, 1, w_pos_gps_v);
inertial_filter_correct(gps_corr[1][1], dt, y_est, 1, w_pos_gps_v);
inertial_filter_correct(corr_gps[0][1], dt, x_est, 1, w_xy_gps_v);
inertial_filter_correct(corr_gps[1][1], dt, y_est, 1, w_xy_gps_v);
}
}
if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
warnx("BAD ESTIMATE AFTER CORRECTION dt: %.6f x: %.3f %.3f %.3f y: %.3f %.3f %.3f", dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2]);
warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", accel_corr[0], accel_corr[1], gps_corr[0][0], gps_corr[0][1], gps_corr[1][0], gps_corr[1][1]);
warnx("BAD ESTIMATE AFTER CORRECTION acc: %.3f %.3f gps x: %.3f %.3f gps y: %.3f %.3f", corr_acc[0], corr_acc[1], corr_gps[0][0], corr_gps[0][1], corr_gps[1][0], corr_gps[1][1]);
thread_should_exit = true;
}
}
@@ -775,9 +801,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (t > pub_last + pub_interval) {
pub_last = t;
/* publish local position */
local_pos.xy_valid = can_estimate_xy && use_gps;
local_pos.xy_valid = can_estimate_xy && use_gps_xy;
local_pos.v_xy_valid = can_estimate_xy;
local_pos.xy_global = local_pos.xy_valid && use_gps; // will make sense when local position sources (e.g. vicon) will be implemented
local_pos.xy_global = local_pos.xy_valid && use_gps_xy;
local_pos.z_global = local_pos.z_valid && use_gps_z;
local_pos.x = x_est[0];
local_pos.vx = x_est[1];
local_pos.y = y_est[0];
src/modules/position_estimator_inav/position_estimator_inav_params.c
+27 -21
View File
@@ -40,13 +40,15 @@
#include "position_estimator_inav_params.h"
PARAM_DEFINE_FLOAT(INAV_W_ALT_BARO, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_ALT_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_POS_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.5f);
PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_V, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 10.0f);
PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.0f);
PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.0165f);
@@ -59,13 +61,15 @@ PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.3f);
int parameters_init(struct position_estimator_inav_param_handles *h)
{
h->w_alt_baro = param_find("INAV_W_ALT_BARO");
h->w_alt_acc = param_find("INAV_W_ALT_ACC");
h->w_alt_sonar = param_find("INAV_W_ALT_SONAR");
h->w_pos_gps_p = param_find("INAV_W_POS_GPS_P");
h->w_pos_gps_v = param_find("INAV_W_POS_GPS_V");
h->w_pos_acc = param_find("INAV_W_POS_ACC");
h->w_pos_flow = param_find("INAV_W_POS_FLOW");
h->w_z_baro = param_find("INAV_W_Z_BARO");
h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
h->w_z_gps_v = param_find("INAV_W_Z_GPS_V");
h->w_z_acc = param_find("INAV_W_Z_ACC");
h->w_z_sonar = param_find("INAV_W_Z_SONAR");
h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
h->w_xy_acc = param_find("INAV_W_XY_ACC");
h->w_xy_flow = param_find("INAV_W_XY_FLOW");
h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
h->flow_k = param_find("INAV_FLOW_K");
@@ -81,13 +85,15 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
int parameters_update(const struct position_estimator_inav_param_handles *h, struct position_estimator_inav_params *p)
{
param_get(h->w_alt_baro, &(p->w_alt_baro));
param_get(h->w_alt_acc, &(p->w_alt_acc));
param_get(h->w_alt_sonar, &(p->w_alt_sonar));
param_get(h->w_pos_gps_p, &(p->w_pos_gps_p));
param_get(h->w_pos_gps_v, &(p->w_pos_gps_v));
param_get(h->w_pos_acc, &(p->w_pos_acc));
param_get(h->w_pos_flow, &(p->w_pos_flow));
param_get(h->w_z_baro, &(p->w_z_baro));
param_get(h->w_z_gps_p, &(p->w_z_gps_p));
param_get(h->w_z_gps_v, &(p->w_z_gps_v));
param_get(h->w_z_acc, &(p->w_z_acc));
param_get(h->w_z_sonar, &(p->w_z_sonar));
param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
param_get(h->w_xy_acc, &(p->w_xy_acc));
param_get(h->w_xy_flow, &(p->w_xy_flow));
param_get(h->w_gps_flow, &(p->w_gps_flow));
param_get(h->w_acc_bias, &(p->w_acc_bias));
param_get(h->flow_k, &(p->flow_k));
src/modules/position_estimator_inav/position_estimator_inav_params.h
+18 -14
View File
@@ -41,13 +41,15 @@
#include <systemlib/param/param.h>
struct position_estimator_inav_params {
float w_alt_baro;
float w_alt_acc;
float w_alt_sonar;
float w_pos_gps_p;
float w_pos_gps_v;
float w_pos_acc;
float w_pos_flow;
float w_z_baro;
float w_z_gps_p;
float w_z_gps_v;
float w_z_acc;
float w_z_sonar;
float w_xy_gps_p;
float w_xy_gps_v;
float w_xy_acc;
float w_xy_flow;
float w_gps_flow;
float w_acc_bias;
float flow_k;
@@ -60,13 +62,15 @@ struct position_estimator_inav_params {
};
struct position_estimator_inav_param_handles {
param_t w_alt_baro;
param_t w_alt_acc;
param_t w_alt_sonar;
param_t w_pos_gps_p;
param_t w_pos_gps_v;
param_t w_pos_acc;
param_t w_pos_flow;
param_t w_z_baro;
param_t w_z_gps_p;
param_t w_z_gps_v;
param_t w_z_acc;
param_t w_z_sonar;
param_t w_xy_gps_p;
param_t w_xy_gps_v;
param_t w_xy_acc;
param_t w_xy_flow;
param_t w_gps_flow;
param_t w_acc_bias;
param_t flow_k;
src/modules/sdlog2/sdlog2.c
+106 -27
View File
@@ -58,6 +58,7 @@
#include <systemlib/err.h>
#include <unistd.h>
#include <drivers/drv_hrt.h>
#include <math.h>
#include <uORB/uORB.h>
#include <uORB/topics/vehicle_status.h>
@@ -84,12 +85,14 @@
#include <uORB/topics/esc_status.h>
#include <systemlib/systemlib.h>
#include <systemlib/param/param.h>
#include <mavlink/mavlink_log.h>
#include "logbuffer.h"
#include "sdlog2_format.h"
#include "sdlog2_messages.h"
#include "sdlog2_version.h"
#define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
log_msgs_written++; \
@@ -180,8 +183,17 @@ static void sdlog2_stop_log(void);
/**
* Write a header to log file: list of message formats.
*/
static void write_formats(int fd);
static int write_formats(int fd);
/**
* Write version message to log file.
*/
static int write_version(int fd);
/**
* Write parameters to log file.
*/
static int write_parameters(int fd);
static bool file_exist(const char *filename);
@@ -237,11 +249,11 @@ int sdlog2_main(int argc, char *argv[])
main_thread_should_exit = false;
deamon_task = task_spawn_cmd("sdlog2",
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
SCHED_DEFAULT,
SCHED_PRIORITY_DEFAULT - 30,
3000,
sdlog2_thread_main,
(const char **)argv);
exit(0);
}
@@ -354,10 +366,13 @@ static void *logwriter_thread(void *arg)
struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
int log_file = open_logfile();
int log_fd = open_logfile();
/* write log messages formats */
write_formats(log_file);
/* write log messages formats, version and parameters */
log_bytes_written += write_formats(log_fd);
log_bytes_written += write_version(log_fd);
log_bytes_written += write_parameters(log_fd);
fsync(log_fd);
int poll_count = 0;
@@ -396,7 +411,7 @@ static void *logwriter_thread(void *arg)
n = available;
}
n = write(log_file, read_ptr, n);
n = write(log_fd, read_ptr, n);
should_wait = (n == available) && !is_part;
@@ -411,21 +426,23 @@ static void *logwriter_thread(void *arg)
} else {
n = 0;
/* exit only with empty buffer */
if (main_thread_should_exit || logwriter_should_exit) {
break;
}
should_wait = true;
}
if (++poll_count == 10) {
fsync(log_file);
fsync(log_fd);
poll_count = 0;
}
}
fsync(log_file);
close(log_file);
fsync(log_fd);
close(log_fd);
return OK;
}
@@ -479,34 +496,92 @@ void sdlog2_stop_log()
/* wait for write thread to return */
int ret;
if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
warnx("error joining logwriter thread: %i", ret);
}
logwriter_pthread = 0;
sdlog2_status();
}
void write_formats(int fd)
int write_formats(int fd)
{
/* construct message format packet */
struct {
LOG_PACKET_HEADER;
struct log_format_s body;
} log_format_packet = {
} log_msg_format = {
LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
};
/* fill message format packet for each format and write to log */
int i;
int written = 0;
for (i = 0; i < log_formats_num; i++) {
log_format_packet.body = log_formats[i];
log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet));
/* fill message format packet for each format and write it */
for (int i = 0; i < log_formats_num; i++) {
log_msg_format.body = log_formats[i];
written += write(fd, &log_msg_format, sizeof(log_msg_format));
}
fsync(fd);
return written;
}
int write_version(int fd)
{
/* construct version message */
struct {
LOG_PACKET_HEADER;
struct log_VER_s body;
} log_msg_VER = {
LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
};
/* fill version message and write it */
strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
return write(fd, &log_msg_VER, sizeof(log_msg_VER));
}
int write_parameters(int fd)
{
/* construct parameter message */
struct {
LOG_PACKET_HEADER;
struct log_PARM_s body;
} log_msg_PARM = {
LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
};
int written = 0;
param_t params_cnt = param_count();
for (param_t param = 0; param < params_cnt; param++) {
/* fill parameter message and write it */
strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
float value = NAN;
switch (param_type(param)) {
case PARAM_TYPE_INT32: {
int32_t i;
param_get(param, &i);
value = i; // cast integer to float
break;
}
case PARAM_TYPE_FLOAT:
param_get(param, &value);
break;
default:
break;
}
log_msg_PARM.body.value = value;
written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
}
return written;
}
int sdlog2_thread_main(int argc, char *argv[])
@@ -584,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[])
}
const char *converter_in = "/etc/logging/conv.zip";
char* converter_out = malloc(120);
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 */
@@ -603,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[])
}
struct vehicle_status_s buf_status;
memset(&buf_status, 0, sizeof(buf_status));
/* warning! using union here to save memory, elements should be used separately! */
@@ -628,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[])
struct esc_status_s esc;
struct vehicle_global_velocity_setpoint_s global_vel_sp;
} buf;
memset(&buf, 0, sizeof(buf));
struct {
@@ -799,7 +877,7 @@ int sdlog2_thread_main(int argc, char *argv[])
subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
fds[fdsc_count].fd = subs.airspeed_sub;
fds[fdsc_count].events = POLLIN;
fdsc_count++;
fdsc_count++;
/* --- ESCs --- */
subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
@@ -890,7 +968,7 @@ int sdlog2_thread_main(int argc, char *argv[])
continue;
}
ifds = 1; // Begin from fds[1] again
ifds = 1; // begin from fds[1] again
pthread_mutex_lock(&logbuffer_mutex);
@@ -1160,8 +1238,8 @@ int sdlog2_thread_main(int argc, char *argv[])
/* --- ESCs --- */
if (fds[ifds++].revents & POLLIN) {
orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
for (uint8_t i=0; i<buf.esc.esc_count; i++)
{
for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
log_msg.msg_type = LOG_ESC_MSG;
log_msg.body.log_ESC.counter = buf.esc.counter;
log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
@@ -1309,6 +1387,7 @@ void handle_status(struct vehicle_status_s *status)
{
// TODO use flag from actuator_armed here?
bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
if (armed != flag_system_armed) {
flag_system_armed = armed;
src/modules/sdlog2/sdlog2_format.h
+4 -4
View File
@@ -85,10 +85,10 @@ struct log_format_s {
#define LOG_FORMAT(_name, _format, _labels) { \
.type = LOG_##_name##_MSG, \
.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
.name = #_name, \
.format = _format, \
.labels = _labels \
}
#define LOG_FORMAT_MSG 0x80
src/modules/sdlog2/sdlog2_messages.h
+29 -29
View File
@@ -48,12 +48,6 @@
/* define message formats */
#pragma pack(push, 1)
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 1
struct log_TIME_s {
uint64_t t;
};
/* --- ATT - ATTITUDE --- */
#define LOG_ATT_MSG 2
struct log_ATT_s {
@@ -253,38 +247,41 @@ struct log_GVSP_s {
float vz;
};
/* --- FWRV - FIRMWARE REVISION --- */
#define LOG_FWRV_MSG 20
struct log_FWRV_s {
char fw_revision[64];
};
/* --- DIST - DISTANCE TO SURFACE --- */
#define LOG_DIST_MSG 21
#define LOG_DIST_MSG 20
struct log_DIST_s {
float bottom;
float bottom_rate;
uint8_t flags;
};
/********** SYSTEM MESSAGES, ID > 0x80 **********/
/* --- TIME - TIME STAMP --- */
#define LOG_TIME_MSG 129
struct log_TIME_s {
uint64_t t;
};
/* --- VER - VERSION --- */
#define LOG_VER_MSG 130
struct log_VER_s {
char arch[16];
char fw_git[64];
};
/* --- PARM - PARAMETER --- */
#define LOG_PARM_MSG 131
struct log_PARM_s {
char name[16];
float value;
};
#pragma pack(pop)
/*
GIT_VERSION is defined at build time via a Makefile call to the
git command line. We create a fake log message format for
the firmware revision "FWRV" that is written to every log
header. This makes it easier to determine which version
of the firmware was running when a log was created.
*/
#define FREEZE_STR(s) #s
#define STRINGIFY(s) FREEZE_STR(s)
#define FW_VERSION_STR STRINGIFY(GIT_VERSION)
/* construct list of all message formats */
static const struct log_format_s log_formats[] = {
LOG_FORMAT(TIME, "Q", "StartTime"),
/* business-level messages, ID < 0x80 */
LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
@@ -303,8 +300,11 @@ static const struct log_format_s log_formats[] = {
LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
LOG_FORMAT(FWRV, "Z", FW_VERSION_STR),
LOG_FORMAT(DIST, "ffB", "Bottom,BottomRate,Flags"),
/* system-level messages, ID >= 0x80 */
// FMT: don't write format of format message, it's useless
LOG_FORMAT(TIME, "Q", "StartTime"),
LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
LOG_FORMAT(PARM, "Nf", "Name,Value"),
};
static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
src/modules/sdlog2/sdlog2_version.h
+62
View File
@@ -0,0 +1,62 @@
/****************************************************************************
*
* 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 sdlog2_version.h
*
* Tools for system version detection.
*
* @author Anton Babushkin <anton.babushkin@me.com>
*/
#ifndef SDLOG2_VERSION_H_
#define SDLOG2_VERSION_H_
/*
GIT_VERSION is defined at build time via a Makefile call to the
git command line.
*/
#define FREEZE_STR(s) #s
#define STRINGIFY(s) FREEZE_STR(s)
#define FW_GIT STRINGIFY(GIT_VERSION)
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
#define HW_ARCH "PX4FMU_V1"
#endif
#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
#define HW_ARCH "PX4FMU_V2"
#endif
#endif /* SDLOG2_VERSION_H_ */
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
+29 -2
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);
@@ -194,8 +220,9 @@ PARAM_DEFINE_INT32(RC_MAP_DIST_B_SW, 0);
PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera yaw / azimuth */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera pitch / tilt */
PARAM_DEFINE_INT32(RC_MAP_AUX1, 0); /**< default function: camera pitch */
PARAM_DEFINE_INT32(RC_MAP_AUX2, 0); /**< default function: camera roll */
PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); /**< default function: camera azimuth / yaw */
PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);
src/modules/sensors/sensors.cpp
+23 -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>
@@ -74,6 +75,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/rc_channels.h>
#include <uORB/topics/manual_control_setpoint.h>
#include <uORB/topics/actuator_controls.h>
#include <uORB/topics/vehicle_control_mode.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/battery_status.h>
@@ -135,75 +137,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
*
@@ -264,6 +197,7 @@ private:
orb_advert_t _sensor_pub; /**< combined sensor data topic */
orb_advert_t _manual_control_pub; /**< manual control signal topic */
orb_advert_t _actuator_group_3_pub; /**< manual control as actuator topic */
orb_advert_t _rc_pub; /**< raw r/c control topic */
orb_advert_t _battery_pub; /**< battery status */
orb_advert_t _airspeed_pub; /**< airspeed */
@@ -386,11 +320,6 @@ private:
*/
int parameters_update();
/**
* Get the rotation matrices
*/
void get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
/**
* Do accel-related initialisation.
*/
@@ -521,6 +450,7 @@ Sensors::Sensors() :
/* publications */
_sensor_pub(-1),
_manual_control_pub(-1),
_actuator_group_3_pub(-1),
_rc_pub(-1),
_battery_pub(-1),
_airspeed_pub(-1),
@@ -811,24 +741,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()
{
@@ -1326,6 +1238,7 @@ Sensors::rc_poll()
orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
struct manual_control_setpoint_s manual_control;
struct actuator_controls_s actuator_group_3;
/* initialize to default values */
manual_control.roll = NAN;
@@ -1502,6 +1415,16 @@ Sensors::rc_poll()
manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
}
/* copy from mapped manual control to control group 3 */
actuator_group_3.control[0] = manual_control.roll;
actuator_group_3.control[1] = manual_control.pitch;
actuator_group_3.control[2] = manual_control.yaw;
actuator_group_3.control[3] = manual_control.throttle;
actuator_group_3.control[4] = manual_control.flaps;
actuator_group_3.control[5] = manual_control.aux1;
actuator_group_3.control[6] = manual_control.aux2;
actuator_group_3.control[7] = manual_control.aux3;
/* check if ready for publishing */
if (_rc_pub > 0) {
orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
@@ -1518,6 +1441,14 @@ Sensors::rc_poll()
} else {
_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
}
/* check if ready for publishing */
if (_actuator_group_3_pub > 0) {
orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
} else {
_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
}
}
}
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