forked from Archive/PX4-Autopilot
commander: Reject mag samples which are on top of each other
This commit is contained in:
parent
640024357f
commit
4fadb65ac6
|
@ -65,6 +65,8 @@ static const int ERROR = -1;
|
||||||
|
|
||||||
static const char *sensor_name = "mag";
|
static const char *sensor_name = "mag";
|
||||||
static const unsigned max_mags = 3;
|
static const unsigned max_mags = 3;
|
||||||
|
static constexpr float mag_sphere_radius = 0.2f;
|
||||||
|
static const unsigned int calibration_sides = 3;
|
||||||
|
|
||||||
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
|
calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
|
||||||
|
|
||||||
|
@ -76,7 +78,7 @@ typedef struct {
|
||||||
unsigned int calibration_points_perside;
|
unsigned int calibration_points_perside;
|
||||||
unsigned int calibration_interval_perside_seconds;
|
unsigned int calibration_interval_perside_seconds;
|
||||||
uint64_t calibration_interval_perside_useconds;
|
uint64_t calibration_interval_perside_useconds;
|
||||||
unsigned int calibration_counter_total;
|
unsigned int calibration_counter_total[max_mags];
|
||||||
bool side_data_collected[detect_orientation_side_count];
|
bool side_data_collected[detect_orientation_side_count];
|
||||||
float* x[max_mags];
|
float* x[max_mags];
|
||||||
float* y[max_mags];
|
float* y[max_mags];
|
||||||
|
@ -184,6 +186,25 @@ int do_mag_calibration(int mavlink_fd)
|
||||||
return result;
|
return result;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, unsigned max_count)
|
||||||
|
{
|
||||||
|
float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f;
|
||||||
|
//float min_sample_dist = (2.0f * M_PI_F * mag_sphere_radius / max_count) / 2.0f;
|
||||||
|
|
||||||
|
for (size_t i = 0; i < count; i++) {
|
||||||
|
float dx = sx - x[i];
|
||||||
|
float dy = sy - y[i];
|
||||||
|
float dz = sz - z[i];
|
||||||
|
float dist = sqrtf(dx * dx + dy * dy + dz * dz);
|
||||||
|
|
||||||
|
if (dist < min_sample_dist) {
|
||||||
|
return true;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
|
static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
|
||||||
{
|
{
|
||||||
calibrate_return result = calibrate_return_ok;
|
calibrate_return result = calibrate_return_ok;
|
||||||
|
@ -286,27 +307,47 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
|
||||||
int poll_ret = poll(fds, fd_count, 1000);
|
int poll_ret = poll(fds, fd_count, 1000);
|
||||||
|
|
||||||
if (poll_ret > 0) {
|
if (poll_ret > 0) {
|
||||||
|
|
||||||
|
int prev_count[max_mags];
|
||||||
|
bool rejected = false;
|
||||||
|
|
||||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||||
|
|
||||||
|
prev_count[cur_mag] = worker_data->calibration_counter_total[cur_mag];
|
||||||
|
|
||||||
if (worker_data->sub_mag[cur_mag] >= 0) {
|
if (worker_data->sub_mag[cur_mag] >= 0) {
|
||||||
struct mag_report mag;
|
struct mag_report mag;
|
||||||
|
|
||||||
orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
|
orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag);
|
||||||
|
|
||||||
|
// Check if this measurement is good to go in
|
||||||
|
rejected = rejected || reject_sample(mag.x, mag.y, mag.z,
|
||||||
|
worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag],
|
||||||
|
worker_data->calibration_counter_total[cur_mag],
|
||||||
|
calibration_sides * worker_data->calibration_points_perside);
|
||||||
|
|
||||||
worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x;
|
worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
|
||||||
worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
|
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
|
||||||
worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
|
worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.z;
|
||||||
|
worker_data->calibration_counter_total[cur_mag]++;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
worker_data->calibration_counter_total++;
|
// Keep calibration of all mags in lockstep
|
||||||
calibration_counter_side++;
|
if (rejected) {
|
||||||
|
// Reset counts, since one of the mags rejected the measurement
|
||||||
// Progress indicator for side
|
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||||
mavlink_and_console_log_info(worker_data->mavlink_fd,
|
worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
|
||||||
"[cal] %s side calibration: progress <%u>",
|
}
|
||||||
detect_orientation_str(orientation),
|
} else {
|
||||||
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
|
calibration_counter_side++;
|
||||||
|
|
||||||
|
// Progress indicator for side
|
||||||
|
mavlink_and_console_log_info(worker_data->mavlink_fd,
|
||||||
|
"[cal] %s side calibration: progress <%u>",
|
||||||
|
detect_orientation_str(orientation),
|
||||||
|
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
|
||||||
|
}
|
||||||
} else {
|
} else {
|
||||||
poll_errcount++;
|
poll_errcount++;
|
||||||
}
|
}
|
||||||
|
@ -336,7 +377,6 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
||||||
|
|
||||||
worker_data.mavlink_fd = mavlink_fd;
|
worker_data.mavlink_fd = mavlink_fd;
|
||||||
worker_data.done_count = 0;
|
worker_data.done_count = 0;
|
||||||
worker_data.calibration_counter_total = 0;
|
|
||||||
worker_data.calibration_points_perside = 80;
|
worker_data.calibration_points_perside = 80;
|
||||||
worker_data.calibration_interval_perside_seconds = 20;
|
worker_data.calibration_interval_perside_seconds = 20;
|
||||||
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
|
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
|
||||||
|
@ -357,9 +397,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
||||||
worker_data.x[cur_mag] = NULL;
|
worker_data.x[cur_mag] = NULL;
|
||||||
worker_data.y[cur_mag] = NULL;
|
worker_data.y[cur_mag] = NULL;
|
||||||
worker_data.z[cur_mag] = NULL;
|
worker_data.z[cur_mag] = NULL;
|
||||||
|
worker_data.calibration_counter_total[cur_mag] = 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
const unsigned int calibration_sides = 3;
|
|
||||||
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
|
const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside;
|
||||||
|
|
||||||
char str[30];
|
char str[30];
|
||||||
|
@ -438,7 +478,7 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
||||||
// Mag in this slot is available and we should have values for it to calibrate
|
// Mag in this slot is available and we should have values for it to calibrate
|
||||||
|
|
||||||
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
|
sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag],
|
||||||
worker_data.calibration_counter_total,
|
worker_data.calibration_counter_total[cur_mag],
|
||||||
100, 0.0f,
|
100, 0.0f,
|
||||||
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
|
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag],
|
||||||
&sphere_radius[cur_mag]);
|
&sphere_radius[cur_mag]);
|
||||||
|
@ -450,6 +490,41 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
// Print uncalibrated data points
|
||||||
|
if (result == calibrate_return_ok) {
|
||||||
|
|
||||||
|
printf("RAW DATA:\n--------------------\n");
|
||||||
|
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||||
|
|
||||||
|
printf("RAW: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
|
||||||
|
|
||||||
|
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
|
||||||
|
float x = worker_data.x[cur_mag][i];
|
||||||
|
float y = worker_data.y[cur_mag][i];
|
||||||
|
float z = worker_data.z[cur_mag][i];
|
||||||
|
printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
|
||||||
|
}
|
||||||
|
|
||||||
|
printf(">>>>>>>\n");
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("CALIBRATED DATA:\n--------------------\n");
|
||||||
|
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
|
||||||
|
|
||||||
|
printf("Calibrated: MAG %u with %u samples:\n", cur_mag, worker_data.calibration_counter_total[cur_mag]);
|
||||||
|
|
||||||
|
for (size_t i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) {
|
||||||
|
float x = worker_data.x[cur_mag][i] - sphere_x[cur_mag];
|
||||||
|
float y = worker_data.y[cur_mag][i] - sphere_y[cur_mag];
|
||||||
|
float z = worker_data.z[cur_mag][i] - sphere_z[cur_mag];
|
||||||
|
printf("%8.4f, %8.4f, %8.4f\n", (double)x, (double)y, (double)z);
|
||||||
|
}
|
||||||
|
|
||||||
|
printf("SPHERE RADIUS: %8.4f", (double)sphere_radius[cur_mag]);
|
||||||
|
printf(">>>>>>>\n");
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
// Data points are no longer needed
|
// Data points are no longer needed
|
||||||
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
|
||||||
|
|
Loading…
Reference in New Issue