diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-19 00:39:06 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-19 00:39:06 +0200 |
commit | d3ae83cb2283591e56ad2999271f7bbd6c756a0d (patch) | |
tree | 90ed454ec664c29022660366205a261a61f85cbf /apps/commander | |
parent | 97726fa67904650c8d82ec7da58924f261deb125 (diff) | |
parent | 5ec5754f26f1059847b7f149bcbdf2b0d11a5115 (diff) | |
download | px4-firmware-d3ae83cb2283591e56ad2999271f7bbd6c756a0d.tar.gz px4-firmware-d3ae83cb2283591e56ad2999271f7bbd6c756a0d.tar.bz2 px4-firmware-d3ae83cb2283591e56ad2999271f7bbd6c756a0d.zip |
Merge branch 'daregger_rate_control' of github.com:PX4/Firmware into calibration
Diffstat (limited to 'apps/commander')
-rw-r--r-- | apps/commander/commander.c | 57 |
1 files changed, 34 insertions, 23 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f11c583d4..545569a65 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -292,10 +292,16 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) struct sensor_combined_s raw; /* 30 seconds */ - uint64_t calibration_interval = 30 * 1000 * 1000; + uint64_t calibration_interval = 45 * 1000 * 1000; unsigned int calibration_counter = 0; - float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN}; + /* + * FLT_MIN is not the most negative float number, + * but the smallest number by magnitude float can + * represent. Use -FLT_MAX to initialize the most + * negative number + */ + float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); @@ -322,8 +328,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) { @@ -331,18 +337,21 @@ 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)) { - continue; + break; } // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); @@ -358,26 +367,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 { @@ -840,7 +849,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "[commander] CMD starting accel calibration"); + ioctl(buzzer, TONE_SET_ALARM, 2); do_accel_calibration(status_pub, ¤t_status); + ioctl(buzzer, TONE_SET_ALARM, 2); mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration"); do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); result = MAV_RESULT_ACCEPTED; |