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

commander: Reject mag samples which are on top of each other

This commit is contained in:
Lorenz Meier 2015-06-24 12:21:40 +02:00
parent 640024357f
commit 4fadb65ac6
1 changed files with 92 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -65,6 +65,8 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
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]);
@ -76,7 +78,7 @@ typedef struct {
unsigned int calibration_points_perside;
unsigned int calibration_interval_perside_seconds;
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];
float* x[max_mags];
float* y[max_mags];
@ -184,6 +186,25 @@ int do_mag_calibration(int mavlink_fd)
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)
{
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);
if (poll_ret > 0) {
int prev_count[max_mags];
bool rejected = false;
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) {
struct mag_report 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->y[cur_mag][worker_data->calibration_counter_total] = mag.y;
worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z;
worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.x;
worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = mag.y;
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++;
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)));
// Keep calibration of all mags in lockstep
if (rejected) {
// Reset counts, since one of the mags rejected the measurement
for (size_t cur_mag = 0; cur_mag < max_mags; cur_mag++) {
worker_data->calibration_counter_total[cur_mag] = prev_count[cur_mag];
}
} else {
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 {
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.done_count = 0;
worker_data.calibration_counter_total = 0;
worker_data.calibration_points_perside = 80;
worker_data.calibration_interval_perside_seconds = 20;
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.y[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;
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
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,
&sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[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
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {