fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-16 07:03:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Calibration message cleanup

Browse Source
This commit is contained in:
Lorenz Meier
2013-08-27 09:48:22 +02:00
parent 9f45b1c589
commit 94d8ec4a1c
3 changed files with 35 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/accelerometer_calibration.cpp
+8 -9
View File
@@ -219,7 +219,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
bool done = true;
char str[60];
int str_ptr;
str_ptr = sprintf(str, "keep vehicle still:");
str_ptr = sprintf(str, "keep the vehicle still in one of these axes:");
unsigned old_done_count = done_count;
done_count = 0;
for (int i = 0; i < 6; i++) {
@@ -236,22 +236,21 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
if (done)
break;
mavlink_log_info(mavlink_fd, str);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0)
return ERROR;
if (data_collected[orient]) {
sprintf(str, "%s direction already measured, please rotate", orientation_strs[orient]);
sprintf(str, "%s done, please rotate to a different axis", orientation_strs[orient]);
mavlink_log_info(mavlink_fd, str);
continue;
}
// sprintf(str,
mavlink_log_info(mavlink_fd, "accel meas started: %s", orientation_strs[orient]);
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);
str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient],
str_ptr = sprintf(str, "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]);
@@ -265,7 +264,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
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 calc error");
mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
return ERROR;
}
@@ -337,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
/* is still now */
if (t_still == 0) {
/* first time */
mavlink_log_info(mavlink_fd, "still...");
mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
t_still = t;
t_timeout = t + timeout;
} else {
@@ -352,7 +351,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
accel_disp[2] > still_thr2 * 2.0f) {
/* not still, reset still start time */
if (t_still != 0) {
mavlink_log_info(mavlink_fd, "moving...");
mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
t_still = 0;
}
}
@@ -364,7 +363,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: failed reading accel");
mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
return -1;
}
}
src/modules/commander/gyro_calibration.cpp
+10 -5
View File
@@ -58,7 +58,7 @@ static const int ERROR = -1;
int do_gyro_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still");
mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
const int calibration_count = 5000;
@@ -84,6 +84,8 @@ int do_gyro_calibration(int mavlink_fd)
close(fd);
unsigned poll_errcount = 0;
while (calibration_counter < calibration_count) {
/* wait blocking for new data */
@@ -93,16 +95,19 @@ int do_gyro_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
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++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
return ERROR;
}
}
src/modules/commander/mag_calibration.cpp
+17 -8
View File
@@ -61,7 +61,7 @@ static const int ERROR = -1;
int do_mag_calibration(int mavlink_fd)
{
mavlink_log_info(mavlink_fd, "mag calibration starting, hold still");
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;
@@ -123,6 +123,10 @@ int do_mag_calibration(int mavlink_fd)
return ERROR;
}
mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
unsigned poll_errcount = 0;
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter < calibration_maxcount) {
@@ -137,7 +141,7 @@ int do_mag_calibration(int mavlink_fd)
axis_index++;
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
tune_neutral();
@@ -158,7 +162,7 @@ int do_mag_calibration(int mavlink_fd)
int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
if (poll_ret > 0) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x;
@@ -190,11 +194,16 @@ int do_mag_calibration(int mavlink_fd)
calibration_counter++;
} else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
break;
} else {
poll_errcount++;
}
if (poll_errcount > 1000) {
mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
return ERROR;
}
}
float sphere_x;
@@ -276,7 +285,7 @@ int do_mag_calibration(int mavlink_fd)
(double)mscale.y_scale, (double)mscale.z_scale);
mavlink_log_info(mavlink_fd, buf);
mavlink_log_info(mavlink_fd, "mag calibration done");
mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
return OK;