diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-16 11:07:41 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-10-16 11:07:41 +0200 |
commit | a720bfff5e7562870a1f3a82bd2fc8da1d660658 (patch) | |
tree | 792ffe4b24eeeefea94aa0b56dd63a3fdf082082 /apps/commander | |
parent | c1958bdaa916560ea45cb6756fe6da25fe79f091 (diff) | |
parent | 7ccc4f6096c32671b02c6353fcb08fd6d3d7b5ea (diff) | |
download | px4-firmware-a720bfff5e7562870a1f3a82bd2fc8da1d660658.tar.gz px4-firmware-a720bfff5e7562870a1f3a82bd2fc8da1d660658.tar.bz2 px4-firmware-a720bfff5e7562870a1f3a82bd2fc8da1d660658.zip |
Merge branch 'tobi'
Diffstat (limited to 'apps/commander')
-rw-r--r-- | apps/commander/commander.c | 20 |
1 files changed, 15 insertions, 5 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index c569a6aa4..78fac5c22 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -296,7 +296,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) int calibration_interval_ms = 30 * 1000; unsigned int calibration_counter = 0; - float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; + float mag_max[3] = {FLT_MIN, FLT_MIN, FLT_MIN}; float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; int fd = open(MAG_DEVICE_PATH, 0); @@ -353,6 +353,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } } + usleep(200000); + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); /* disable calibration mode */ @@ -377,7 +379,15 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mag_offset[1] = (mag_max[1] + mag_min[1]) / 2.0f; mag_offset[2] = (mag_max[2] + mag_min[2]) / 2.0f; - printf("mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); + if (!isfinite(mag_offset[0]) || !isfinite(mag_offset[1]) || !isfinite(mag_offset[2])) + { + mavlink_log_info(mavlink_fd, "[commander] mag cal aborted: NaN"); + } + + //char buf[52]; + + //sprintf(buf, "mag off x: %4.4f, y: %4.4f, z: %4.4f\n",(double)mag_offset[0],(double)mag_offset[0],(double)mag_offset[2]); + //mavlink_log_info(mavlink_fd, buf); /* announce and set new offset */ @@ -412,7 +422,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: auto-save of params to EEPROM failed"); } - mavlink_log_info(mavlink_fd, "[commander] magnetometer calibration finished"); + mavlink_log_info(mavlink_fd, "[commander] mag cal finished"); close(sub_sensor_combined); } @@ -519,7 +529,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); - const int calibration_count = 5000; + const int calibration_count = 2500; int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s raw; @@ -954,7 +964,7 @@ int commander_main(int argc, char *argv[]) deamon_task = task_spawn("commander", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 50, - 4096, + 8000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); thread_running = true; |