Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Speedup level calibration #13666

Merged
merged 2 commits into from
Dec 3, 2019
Merged

Speedup level calibration #13666

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b8b4245
refactor accelerometer_calibration: using namespace matrix
bkueng Dec 2, 2019
f2ceda8
level calibration: speedup & check for motion
bkueng Dec 3, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
138 changes: 75 additions & 63 deletions src/modules/commander/accelerometer_calibration.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -142,6 +142,7 @@
#include <drivers/drv_hrt.h>
#include <drivers/drv_accel.h>
#include <lib/ecl/geo/geo.h>
#include <matrix/math.hpp>
#include <conversion/rotation.h>
#include <parameters/param.h>
#include <systemlib/err.h>
Expand All @@ -150,6 +151,9 @@
#include <uORB/topics/sensor_correction.h>
#include <uORB/Subscription.hpp>

using namespace time_literals;
using namespace matrix;

static const char *sensor_name = "accel";

static int32_t device_id[max_accel_sens];
Expand Down Expand Up @@ -294,19 +298,19 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub)
int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));
enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
matrix::Dcmf board_rotation = get_rot_matrix(board_rotation_id);
Dcmf board_rotation = get_rot_matrix(board_rotation_id);

matrix::Dcmf board_rotation_t = board_rotation.transpose();
Dcmf board_rotation_t = board_rotation.transpose();

bool tc_locked[3] = {false}; // true when the thermal parameter instance has already been adjusted by the calibrator

for (unsigned uorb_index = 0; uorb_index < active_sensors; uorb_index++) {

/* handle individual sensors, one by one */
matrix::Vector3f accel_offs_vec(accel_offs[uorb_index]);
matrix::Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
matrix::Matrix3f accel_T_mat(accel_T[uorb_index]);
matrix::Matrix3f accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;
Vector3f accel_offs_vec(accel_offs[uorb_index]);
Vector3f accel_offs_rotated = board_rotation_t *accel_offs_vec;
Matrix3f accel_T_mat(accel_T[uorb_index]);
Matrix3f 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);
Expand Down Expand Up @@ -625,15 +629,15 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m
param_get(board_offset_y, &board_offset[1]);
param_get(board_offset_z, &board_offset[2]);

matrix::Dcmf board_rotation_offset = matrix::Eulerf(
M_DEG_TO_RAD_F * board_offset[0],
M_DEG_TO_RAD_F * board_offset[1],
M_DEG_TO_RAD_F * board_offset[2]);
Dcmf board_rotation_offset = Eulerf(
M_DEG_TO_RAD_F * board_offset[0],
M_DEG_TO_RAD_F * board_offset[1],
M_DEG_TO_RAD_F * board_offset[2]);

int32_t board_rotation_int;
param_get(board_rotation_h, &(board_rotation_int));

matrix::Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);
Dcmf board_rotation = board_rotation_offset * get_rot_matrix((enum Rotation)board_rotation_int);

px4_pollfd_struct_t fds[max_accel_sens];

Expand Down Expand Up @@ -713,7 +717,7 @@ calibrate_return read_accelerometer_avg(int sensor_correction_sub, int (&subs)[m

// rotate sensor measurements from sensor to body frame using board rotation matrix
for (unsigned i = 0; i < max_accel_sens; i++) {
matrix::Vector3f accel_sum_vec(&accel_sum[i][0]);
Vector3f accel_sum_vec(&accel_sum[i][0]);
accel_sum_vec = board_rotation * accel_sum_vec;

for (size_t j = 0; j < 3; j++) {
Expand Down Expand Up @@ -793,90 +797,100 @@ calibrate_return calculate_calibration_values(unsigned sensor,

int do_level_calibration(orb_advert_t *mavlink_log_pub)
{
const unsigned cal_time = 5;
const unsigned cal_hz = 100;
unsigned settle_time = 30;

bool success = false;
int att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att));
vehicle_attitude_s att{};

calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, "level");

param_t roll_offset_handle = param_find("SENS_BOARD_X_OFF");
param_t pitch_offset_handle = param_find("SENS_BOARD_Y_OFF");
param_t board_rot_handle = param_find("SENS_BOARD_ROT");

// save old values if calibration fails
// get old values
float roll_offset_current;
float pitch_offset_current;
int32_t board_rot_current = 0;
param_get(roll_offset_handle, &roll_offset_current);
param_get(pitch_offset_handle, &pitch_offset_current);
param_get(board_rot_handle, &board_rot_current);

// give attitude some time to settle if there have been changes to the board rotation parameters
if (board_rot_current == 0 && fabsf(roll_offset_current) < FLT_EPSILON && fabsf(pitch_offset_current) < FLT_EPSILON) {
settle_time = 0;
}

float zero = 0.0f;
param_set_no_notification(roll_offset_handle, &zero);
param_set_no_notification(pitch_offset_handle, &zero);
param_notify_changes();
Dcmf board_rotation_offset = Eulerf(
math::radians(roll_offset_current),
math::radians(pitch_offset_current),
0.f);

px4_pollfd_struct_t fds[1];

fds[0].fd = att_sub;
fds[0].events = POLLIN;

float roll_mean = 0.0f;
float pitch_mean = 0.0f;
unsigned counter = 0;
bool had_motion = true;
int num_retries = 0;

while (had_motion && num_retries++ < 50) {
Vector2f min_angles{100.f, 100.f};
Vector2f max_angles{-100.f, -100.f};
roll_mean = 0.0f;
pitch_mean = 0.0f;
counter = 0;
int last_progress_report = -100;
const hrt_abstime calibration_duration = 500_ms;
const hrt_abstime start = hrt_absolute_time();

while (hrt_elapsed_time(&start) < calibration_duration) {
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);

if (pollret <= 0) {
// attitude estimator is not running
calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
goto out;
}

// sleep for some time
hrt_abstime start = hrt_absolute_time();
int progress = 100 * hrt_elapsed_time(&start) / calibration_duration;

while (hrt_elapsed_time(&start) < settle_time * 1000000) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG,
(int)(90 * hrt_elapsed_time(&start) / 1e6f / (float)settle_time));
px4_sleep(settle_time / 10);
}
if (progress >= last_progress_report + 20) {
calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, progress);
last_progress_report = progress;
}

start = hrt_absolute_time();
orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
Eulerf att_euler = Quatf(att.q);

// average attitude for 5 seconds
while (hrt_elapsed_time(&start) < cal_time * 1000000) {
int pollret = px4_poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
// keep min + max angles
for (int i = 0; i < 2; ++i) {
if (att_euler(i) < min_angles(i)) { min_angles(i) = att_euler(i); }

if (pollret <= 0) {
// attitude estimator is not running
calibration_log_critical(mavlink_log_pub, "attitude estimator not running - check system boot");
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
goto out;
if (att_euler(i) > max_angles(i)) { max_angles(i) = att_euler(i); }
}

att_euler(2) = 0.f; // ignore yaw
att_euler = Eulerf(board_rotation_offset * Dcmf(att_euler)); // subtract existing board rotation
roll_mean += att_euler.phi();
pitch_mean += att_euler.theta();
++counter;
}

orb_copy(ORB_ID(vehicle_attitude), att_sub, &att);
matrix::Eulerf euler = matrix::Quatf(att.q);
roll_mean += euler.phi();
pitch_mean += euler.theta();
counter++;
// motion detection: check that (max-min angle) is within a threshold.
// The difference is typically <0.1 deg while at rest
if (max_angles(0) - min_angles(0) < math::radians(0.5f) &&
max_angles(1) - min_angles(1) < math::radians(0.5f)) {
had_motion = false;
}
}

calibration_log_info(mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 100);

if (counter > (cal_time * cal_hz / 2)) {
roll_mean /= counter;
pitch_mean /= counter;
roll_mean /= counter;
pitch_mean /= counter;

} else {
calibration_log_info(mavlink_log_pub, "not enough measurements taken");
success = false;
goto out;
}
if (had_motion) {
calibration_log_critical(mavlink_log_pub, "motion during calibration");

if (fabsf(roll_mean) > 0.8f) {
} else if (fabsf(roll_mean) > 0.8f) {
calibration_log_critical(mavlink_log_pub, "excess roll angle");

} else if (fabsf(pitch_mean) > 0.8f) {
Expand All @@ -893,15 +907,13 @@ int do_level_calibration(orb_advert_t *mavlink_log_pub)

out:

orb_unsubscribe(att_sub);

if (success) {
calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, "level");
return 0;

} else {
// set old parameters
param_set_no_notification(roll_offset_handle, &roll_offset_current);
param_set_no_notification(pitch_offset_handle, &pitch_offset_current);
param_notify_changes();
calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "level");
return 1;
}
Expand Down