dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: Improve calibration routines to produce more conscise and better sequenced instructions

This commit is contained in:
Lorenz Meier 2014-11-02 21:24:50 +01:00
parent d610564fd0
commit b51c669344
4 changed files with 54 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
src/modules/commander/accelerometer_calibration.cpp
View File

@ -83,7 +83,7 @@
* | accel_T[1][i] | * | accel_T[1][i] |
* [ accel_T[2][i] ] * [ accel_T[2][i] ]
* *
* b = [ accel_corr_ref[0][i] ] // One measurement per axis is enough * b = [ accel_corr_ref[0][i] ] // One measurement per side is enough
* | accel_corr_ref[2][i] | * | accel_corr_ref[2][i] |
* [ accel_corr_ref[4][i] ] * [ accel_corr_ref[4][i] ]
* *
@ -162,6 +162,11 @@ int do_accel_calibration(int mavlink_fd)
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "You need to put the system on all six sides");
sleep(3);
mavlink_log_info(mavlink_fd, "Follow the instructions on the screen");
sleep(5);
struct accel_scale accel_scale = { struct accel_scale accel_scale = {
0.0f, 0.0f,
1.0f, 1.0f,
@ -258,7 +263,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
const int samples_num = 2500; const int samples_num = 2500;
float accel_ref[6][3]; float accel_ref[6][3];
bool data_collected[6] = { false, false, false, false, false, false }; bool data_collected[6] = { false, false, false, false, false, false };
const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" }; const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined)); int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
@ -287,29 +292,37 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
break; break;
} }
mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s", /* inform user which axes are still needed */
(!data_collected[0]) ? "x+ " : "", mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
(!data_collected[1]) ? "x- " : "", (!data_collected[0]) ? "front " : "",
(!data_collected[2]) ? "y+ " : "", (!data_collected[1]) ? "back " : "",
(!data_collected[3]) ? "y- " : "", (!data_collected[2]) ? "left " : "",
(!data_collected[4]) ? "z+ " : "", (!data_collected[3]) ? "right " : "",
(!data_collected[5]) ? "z- " : ""); (!data_collected[4]) ? "up " : "",
(!data_collected[5]) ? "down " : "");
/* allow user enough time to read the message */
sleep(3);
int orient = detect_orientation(mavlink_fd, sensor_combined_sub); int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
if (orient < 0) { if (orient < 0) {
res = ERROR; mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
break; sleep(3);
}
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
continue; continue;
} }
mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]); /* inform user about already handled side */
if (data_collected[orient]) {
mavlink_log_info(mavlink_fd, "%s side done, rotate to a different side", orientation_strs[orient]);
sleep(4);
continue;
}
mavlink_log_info(mavlink_fd, "Hold still, starting to measure %s side", orientation_strs[orient]);
sleep(1);
read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); 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], mavlink_log_info(mavlink_fd, "result for %s side: [ %.2f %.2f %.2f ]", orientation_strs[orient],
(double)accel_ref[orient][0], (double)accel_ref[orient][0],
(double)accel_ref[orient][1], (double)accel_ref[orient][1],
(double)accel_ref[orient][2]); (double)accel_ref[orient][2]);
@ -400,7 +413,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* is still now */ /* is still now */
if (t_still == 0) { if (t_still == 0) {
/* first time */ /* first time */
mavlink_log_info(mavlink_fd, "detected rest position, waiting..."); mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
t_still = t; t_still = t;
t_timeout = t + timeout; t_timeout = t + timeout;
@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
/* not still, reset still start time */ /* not still, reset still start time */
if (t_still != 0) { if (t_still != 0) {
mavlink_log_info(mavlink_fd, "detected motion, hold still..."); mavlink_log_info(mavlink_fd, "detected motion, hold still...");
sleep(3);
t_still = 0; t_still = 0;
} }
} }

25
src/modules/commander/airspeed_calibration.cpp
View File

@ -61,6 +61,15 @@ static const int ERROR = -1;
static const char *sensor_name = "dpress"; static const char *sensor_name = "dpress";
#define HUMAN_ASPD_CAL_FAILED_MSG "Calibration failed, see http://px4.io/help/aspd"
static void feedback_calibration_failed(int mavlink_fd)
{
sleep(5);
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
mavlink_log_critical(mavlink_fd, HUMAN_ASPD_CAL_FAILED_MSG);
}
int do_airspeed_calibration(int mavlink_fd) int do_airspeed_calibration(int mavlink_fd)
{ {
/* give directions */ /* give directions */
@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
float analog_scaling = 0.0f; float analog_scaling = 0.0f;
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling)); param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
if (fabsf(analog_scaling) < 0.1f) { if (fabsf(analog_scaling) < 0.1f) {
mavlink_log_critical(mavlink_fd, "If analog sens, retry with [SENS_DPRES_ANSC=1000]"); mavlink_log_critical(mavlink_fd, "No airspeed sensor, see http://px4.io/help/aspd");
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
} }
} else { } else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) { if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
if (calibration_counter % 500 == 0) { if (calibration_counter % 500 == 0) {
mavlink_log_info(mavlink_fd, "Create airflow! (%d, wanted: 50 Pa)", mavlink_log_info(mavlink_fd, "Create air pressure! (got %d, wanted: 50 Pa)",
(int)diff_pres.differential_pressure_raw_pa); (int)diff_pres.differential_pressure_raw_pa);
} }
continue; continue;
@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
/* do not allow negative values */ /* do not allow negative values */
if (diff_pres.differential_pressure_raw_pa < 0.0f) { if (diff_pres.differential_pressure_raw_pa < 0.0f) {
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)", mavlink_log_info(mavlink_fd, "ERROR: Negative pressure difference detected! (%d Pa)",
(int)diff_pres.differential_pressure_raw_pa); (int)diff_pres.differential_pressure_raw_pa);
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
close(diff_pres_sub); close(diff_pres_sub);
/* the user setup is wrong, wipe the calibration to force a proper re-calibration */ /* the user setup is wrong, wipe the calibration to force a proper re-calibration */
@ -235,7 +244,7 @@ int do_airspeed_calibration(int mavlink_fd)
close(diff_pres_sub); close(diff_pres_sub);
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name); feedback_calibration_failed(mavlink_fd);
return ERROR; return ERROR;
} else { } else {
mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)", mavlink_log_info(mavlink_fd, "Positive pressure: OK (%d Pa)",
@ -245,14 +254,14 @@ int do_airspeed_calibration(int mavlink_fd)
} else if (poll_ret == 0) { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* any poll failure for 1s is a reason to abort */
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }
} }
if (calibration_counter == maxcount) { if (calibration_counter == maxcount) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name); feedback_calibration_failed(mavlink_fd);
close(diff_pres_sub); close(diff_pres_sub);
return ERROR; return ERROR;
} }

5
src/modules/commander/gyro_calibration.cpp
View File

@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
int do_gyro_calibration(int mavlink_fd) int do_gyro_calibration(int mavlink_fd)
{ {
mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
mavlink_log_info(mavlink_fd, "don't move system"); mavlink_log_info(mavlink_fd, "HOLD STILL");
/* wait for the user to respond */
sleep(2);
struct gyro_scale gyro_scale = { struct gyro_scale gyro_scale = {
0.0f, 0.0f,

2
src/modules/commander/mag_calibration.cpp
View File

@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
unsigned poll_errcount = 0; unsigned poll_errcount = 0;
mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis"); mavlink_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down");
calibration_counter = 0; calibration_counter = 0;