diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-17 18:27:49 +0200 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-17 18:27:49 +0200 |
| commit | d1429f266da33aa3355b228adffe56f74a9c3cd1 (patch) | |
| tree | 674c06baf76c6e6ef694ab9085b0dae577cfe2dc | |
| parent | 6a48b91beae575ad4684151f08c75b079caffe5c (diff) | |
| download | px4-firmware-d1429f266da33aa3355b228adffe56f74a9c3cd1.tar.gz px4-firmware-d1429f266da33aa3355b228adffe56f74a9c3cd1.tar.bz2 px4-firmware-d1429f266da33aa3355b228adffe56f74a9c3cd1.zip | |
Calibration progress, needs sphere fitting
| -rw-r--r-- | apps/commander/commander.c | 44 |
1 files changed, 23 insertions, 21 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 746c0e791..687b02d4f 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -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[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[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]; + } calibration_counter++; } else { |