mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-29 03:10:35 +08:00
Merge remote-tracking branch 'upstream/master' into linux
Signed-off-by: Mark Charlebois <charlebm@gmail.com> Conflicts: src/drivers/rgbled/rgbled.cpp src/modules/commander/PreflightCheck.cpp src/modules/commander/airspeed_calibration.cpp src/modules/commander/calibration_routines.cpp src/modules/commander/gyro_calibration.cpp src/modules/commander/mag_calibration.cpp src/modules/mc_att_control/mc_att_control_main.cpp
This commit is contained in:
Mark Charlebois
@@ -153,11 +153,10 @@ static const int ERROR = -1;
|
||||
|
||||
static const char *sensor_name = "accel";
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
|
||||
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
|
||||
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
|
||||
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num);
|
||||
int mat_invert3(float src[3][3], float dst[3][3]);
|
||||
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
|
||||
int accel_calibration_worker(detect_orientation_return orientation, void* worker_data);
|
||||
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g);
|
||||
|
||||
/// Data passed to calibration worker routine
|
||||
typedef struct {
|
||||
@@ -172,7 +171,7 @@ int do_accel_calibration(int mavlink_fd)
|
||||
int fd;
|
||||
int32_t device_id[max_accel_sens];
|
||||
|
||||
mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
|
||||
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
|
||||
|
||||
struct accel_scale accel_scale = {
|
||||
0.0f,
|
||||
@@ -203,7 +202,7 @@ int do_accel_calibration(int mavlink_fd)
|
||||
px4_close(fd);
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, s);
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, s);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -211,13 +210,23 @@ int do_accel_calibration(int mavlink_fd)
|
||||
float accel_T[max_accel_sens][3][3];
|
||||
unsigned active_sensors;
|
||||
|
||||
/* measure and calculate offsets & scales */
|
||||
if (res == OK) {
|
||||
/* measure and calculate offsets & scales */
|
||||
res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
|
||||
calibrate_return cal_return = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T, &active_sensors);
|
||||
if (cal_return == calibrate_return_cancelled) {
|
||||
// Cancel message already displayed, nothing left to do
|
||||
return ERROR;
|
||||
} else if (cal_return == calibrate_return_ok) {
|
||||
res = OK;
|
||||
} else {
|
||||
res = ERROR;
|
||||
}
|
||||
}
|
||||
|
||||
if (res != OK || active_sensors == 0) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
|
||||
if (res != OK) {
|
||||
if (active_sensors == 0) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SENSOR_MSG);
|
||||
}
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -264,7 +273,7 @@ int do_accel_calibration(int mavlink_fd)
|
||||
failed |= (OK != param_set_no_notification(param_find(str), &(device_id[i])));
|
||||
|
||||
if (failed) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, i);
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, i);
|
||||
return ERROR;
|
||||
}
|
||||
|
||||
@@ -272,7 +281,7 @@ int do_accel_calibration(int mavlink_fd)
|
||||
fd = px4_open(str, 0);
|
||||
|
||||
if (fd < 0) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, "sensor does not exist");
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, "sensor does not exist");
|
||||
res = ERROR;
|
||||
} else {
|
||||
res = px4_ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
|
||||
@@ -280,7 +289,7 @@ int do_accel_calibration(int mavlink_fd)
|
||||
}
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, i);
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, i);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -289,41 +298,47 @@ int do_accel_calibration(int mavlink_fd)
|
||||
res = param_save_default();
|
||||
|
||||
if (res != OK) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
|
||||
}
|
||||
|
||||
mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
|
||||
/* if there is a any preflight-check system response, let the barrage of messages through */
|
||||
usleep(200000);
|
||||
|
||||
mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
|
||||
|
||||
} else {
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
|
||||
mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
|
||||
}
|
||||
|
||||
/* give this message enough time to propagate */
|
||||
usleep(600000);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
int accel_calibration_worker(detect_orientation_return orientation, void* data)
|
||||
static calibrate_return accel_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
|
||||
{
|
||||
const unsigned samples_num = 3000;
|
||||
accel_worker_data_t* worker_data = (accel_worker_data_t*)(data);
|
||||
|
||||
mavlink_and_console_log_info(worker_data->mavlink_fd, "Hold still, starting to measure %s side", detect_orientation_str(orientation));
|
||||
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Hold still, measuring %s side", detect_orientation_str(orientation));
|
||||
|
||||
read_accelerometer_avg(worker_data->subs, worker_data->accel_ref, orientation, samples_num);
|
||||
|
||||
mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side result: [ %8.4f %8.4f %8.4f ]", detect_orientation_str(orientation),
|
||||
mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side result: [%8.4f %8.4f %8.4f]", detect_orientation_str(orientation),
|
||||
(double)worker_data->accel_ref[0][orientation][0],
|
||||
(double)worker_data->accel_ref[0][orientation][1],
|
||||
(double)worker_data->accel_ref[0][orientation][2]);
|
||||
|
||||
worker_data->done_count++;
|
||||
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * worker_data->done_count);
|
||||
mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count);
|
||||
|
||||
return OK;
|
||||
return calibrate_return_ok;
|
||||
}
|
||||
|
||||
int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
|
||||
calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors)
|
||||
{
|
||||
int result = OK;
|
||||
calibrate_return result = calibrate_return_ok;
|
||||
|
||||
*active_sensors = 0;
|
||||
|
||||
@@ -344,7 +359,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
|
||||
for (unsigned i = 0; i < max_accel_sens; i++) {
|
||||
worker_data.subs[i] = orb_subscribe_multi(ORB_ID(sensor_accel), i);
|
||||
if (worker_data.subs[i] < 0) {
|
||||
result = ERROR;
|
||||
result = calibrate_return_error;
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -354,8 +369,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
|
||||
timestamps[i] = arp.timestamp;
|
||||
}
|
||||
|
||||
if (result == OK) {
|
||||
result = calibrate_from_orientation(mavlink_fd, data_collected, accel_calibration_worker, &worker_data);
|
||||
if (result == calibrate_return_ok) {
|
||||
int cancel_sub = calibrate_cancel_subscribe();
|
||||
result = calibrate_from_orientation(mavlink_fd, cancel_sub, data_collected, accel_calibration_worker, &worker_data, false /* normal still */);
|
||||
calibrate_cancel_unsubscribe(cancel_sub);
|
||||
}
|
||||
|
||||
/* close all subscriptions */
|
||||
@@ -371,13 +388,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
|
||||
}
|
||||
}
|
||||
|
||||
if (result == OK) {
|
||||
if (result == calibrate_return_ok) {
|
||||
/* calculate offsets and transform matrix */
|
||||
for (unsigned i = 0; i < (*active_sensors); i++) {
|
||||
result = calculate_calibration_values(i, worker_data.accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
|
||||
|
||||
if (result != OK) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, "ERROR: calibration values calculation error");
|
||||
if (result != calibrate_return_ok) {
|
||||
mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: calibration calculation error");
|
||||
break;
|
||||
}
|
||||
}
|
||||
@@ -389,7 +406,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_ac
|
||||
/*
|
||||
* Read specified number of accelerometer samples, calculate average and dispersion.
|
||||
*/
|
||||
int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
|
||||
calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
|
||||
{
|
||||
px4_pollfd_struct_t fds[max_accel_sens];
|
||||
|
||||
@@ -433,7 +450,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
|
||||
}
|
||||
|
||||
if (errcount > samples_num / 10) {
|
||||
return ERROR;
|
||||
return calibrate_return_error;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -443,7 +460,7 @@ int read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_a
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
return calibrate_return_ok;
|
||||
}
|
||||
|
||||
int mat_invert3(float src[3][3], float dst[3][3])
|
||||
@@ -469,7 +486,7 @@ int mat_invert3(float src[3][3], float dst[3][3])
|
||||
return OK;
|
||||
}
|
||||
|
||||
int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
|
||||
calibrate_return calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_sens][detect_orientation_side_count][3], float (&accel_T)[max_accel_sens][3][3], float (&accel_offs)[max_accel_sens][3], float g)
|
||||
{
|
||||
/* calculate offsets */
|
||||
for (unsigned i = 0; i < 3; i++) {
|
||||
@@ -491,7 +508,7 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
|
||||
float mat_A_inv[3][3];
|
||||
|
||||
if (mat_invert3(mat_A, mat_A_inv) != OK) {
|
||||
return ERROR;
|
||||
return calibrate_return_error;
|
||||
}
|
||||
|
||||
/* copy results to accel_T */
|
||||
@@ -502,5 +519,5 @@ int calculate_calibration_values(unsigned sensor, float (&accel_ref)[max_accel_s
|
||||
}
|
||||
}
|
||||
|
||||
return OK;
|
||||
return calibrate_return_ok;
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user