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

Calibration progress, needs sphere fitting

This commit is contained in:
Lorenz Meier 2012-10-17 18:27:49 +02:00
parent 6a48b91bea
commit d1429f266d
1 changed files with 22 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
apps/commander/commander.c
View File

@ -325,8 +325,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
uint64_t axis_deadline = hrt_absolute_time();
uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
const char axislabels[3] = { 'X', 'Y', 'Z'};
int axis_index = 0;
const char axislabels[3] = { 'Z', 'X', 'Y'};
int axis_index = -1;
while (hrt_absolute_time() < calibration_deadline) {
@ -334,15 +334,17 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
/* user guidance */
if (hrt_absolute_time() > axis_deadline &&
if (hrt_absolute_time() >= axis_deadline &&
axis_index < 3) {
axis_index++;
char buf[50];
sprintf(buf, "[commander] Please rotate around %c", axislabels[axis_index]);
mavlink_log_info(mavlink_fd, buf);
ioctl(buzzer, TONE_SET_ALARM, 2);
axis_deadline += calibration_interval / 3;
axis_index++;
}
if (!(axis_index < 3)) {
@ -362,26 +364,26 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* get min/max values */
/* ignore other axes */
if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
mag_min[axis_index] = raw.magnetometer_ga[axis_index];
if (raw.magnetometer_ga[0] < mag_min[0]) {
mag_min[0] = raw.magnetometer_ga[0];
}
else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
mag_max[axis_index] = raw.magnetometer_ga[axis_index];
else if (raw.magnetometer_ga[0] > mag_max[0]) {
mag_max[0] = raw.magnetometer_ga[0];
}
// if (raw.magnetometer_ga[1] < mag_min[1]) {
// mag_min[1] = raw.magnetometer_ga[1];
// }
// else if (raw.magnetometer_ga[1] > mag_max[1]) {
// mag_max[1] = raw.magnetometer_ga[1];
// }
if (raw.magnetometer_ga[1] < mag_min[1]) {
mag_min[1] = raw.magnetometer_ga[1];
}
else if (raw.magnetometer_ga[1] > mag_max[1]) {
mag_max[1] = raw.magnetometer_ga[1];
}
// if (raw.magnetometer_ga[2] < mag_min[2]) {
// mag_min[2] = raw.magnetometer_ga[2];
// }
// else if (raw.magnetometer_ga[2] > mag_max[2]) {
// mag_max[2] = raw.magnetometer_ga[2];
// }
if (raw.magnetometer_ga[2] < mag_min[2]) {
mag_min[2] = raw.magnetometer_ga[2];
}
else if (raw.magnetometer_ga[2] > mag_max[2]) {
mag_max[2] = raw.magnetometer_ga[2];
}
calibration_counter++;
} else {