forked from Archive/PX4-Autopilot
Commander: Improve calibration routines to produce more conscise and better sequenced instructions
This commit is contained in:
parent
d610564fd0
commit
b51c669344
|
@ -83,7 +83,7 @@
|
|||
* | accel_T[1][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[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, "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 = {
|
||||
0.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;
|
||||
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-" };
|
||||
const char *orientation_strs[6] = { "front", "back", "left", "right", "top", "bottom" };
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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- " : "");
|
||||
/* inform user which axes are still needed */
|
||||
mavlink_log_info(mavlink_fd, "pending: %s%s%s%s%s%s",
|
||||
(!data_collected[0]) ? "front " : "",
|
||||
(!data_collected[1]) ? "back " : "",
|
||||
(!data_collected[2]) ? "left " : "",
|
||||
(!data_collected[3]) ? "right " : "",
|
||||
(!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);
|
||||
|
||||
if (orient < 0) {
|
||||
res = ERROR;
|
||||
break;
|
||||
}
|
||||
|
||||
if (data_collected[orient]) {
|
||||
mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
|
||||
mavlink_log_info(mavlink_fd, "invalid motion, hold still...");
|
||||
sleep(3);
|
||||
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);
|
||||
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][1],
|
||||
(double)accel_ref[orient][2]);
|
||||
|
@ -400,7 +413,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, "detected rest position, waiting...");
|
||||
mavlink_log_info(mavlink_fd, "detected rest position, hold still...");
|
||||
t_still = t;
|
||||
t_timeout = t + timeout;
|
||||
|
||||
|
@ -418,6 +431,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
|
|||
/* not still, reset still start time */
|
||||
if (t_still != 0) {
|
||||
mavlink_log_info(mavlink_fd, "detected motion, hold still...");
|
||||
sleep(3);
|
||||
t_still = 0;
|
||||
}
|
||||
}
|
||||
|
|
|
@ -61,6 +61,15 @@ static const int ERROR = -1;
|
|||
|
||||
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)
|
||||
{
|
||||
/* give directions */
|
||||
|
@ -99,7 +108,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
float analog_scaling = 0.0f;
|
||||
param_get(param_find("SENS_DPRES_ANSC"), &(analog_scaling));
|
||||
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);
|
||||
return ERROR;
|
||||
}
|
||||
|
@ -138,7 +147,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
|
||||
} else if (poll_ret == 0) {
|
||||
/* 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);
|
||||
return ERROR;
|
||||
}
|
||||
|
@ -175,7 +184,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
}
|
||||
|
||||
} else {
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
feedback_calibration_failed(mavlink_fd);
|
||||
close(diff_pres_sub);
|
||||
return ERROR;
|
||||
}
|
||||
|
@ -207,7 +216,7 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
|
||||
if (fabsf(diff_pres.differential_pressure_raw_pa) < 50.0f) {
|
||||
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);
|
||||
}
|
||||
continue;
|
||||
|
@ -215,9 +224,9 @@ int do_airspeed_calibration(int mavlink_fd)
|
|||
|
||||
/* do not allow negative values */
|
||||
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)",
|
||||
(int)diff_pres.differential_pressure_raw_pa);
|
||||
mavlink_log_critical(mavlink_fd, "Swap static and dynamic ports!");
|
||||
close(diff_pres_sub);
|
||||
|
||||
/* 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);
|
||||
|
||||
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
feedback_calibration_failed(mavlink_fd);
|
||||
return ERROR;
|
||||
} else {
|
||||
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) {
|
||||
/* 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);
|
||||
return ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (calibration_counter == maxcount) {
|
||||
mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
feedback_calibration_failed(mavlink_fd);
|
||||
close(diff_pres_sub);
|
||||
return ERROR;
|
||||
}
|
||||
|
|
|
@ -63,7 +63,10 @@ static const char *sensor_name = "gyro";
|
|||
int do_gyro_calibration(int mavlink_fd)
|
||||
{
|
||||
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 = {
|
||||
0.0f,
|
||||
|
|
|
@ -155,7 +155,7 @@ int do_mag_calibration(int mavlink_fd)
|
|||
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
|
||||
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;
|
||||
|
||||
|
|
Loading…
Reference in New Issue