diff options
author | Julian Oes <joes@student.ethz.ch> | 2012-09-27 16:50:20 +0200 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2012-09-27 16:50:20 +0200 |
commit | 7f153098926bf977609c6efb53fa7cb5093564af (patch) | |
tree | 9c3ae6daab05346b1e06a2f2e0cb39e0e241cb66 | |
parent | a05c4d050473df87aacc95aadcd978d57f263cbc (diff) | |
download | px4-firmware-7f153098926bf977609c6efb53fa7cb5093564af.tar.gz px4-firmware-7f153098926bf977609c6efb53fa7cb5093564af.tar.bz2 px4-firmware-7f153098926bf977609c6efb53fa7cb5093564af.zip |
Calibration should not freeze anymore, ardrone flying but estimator is not able to use calibrated magnetometer data
-rw-r--r-- | apps/ardrone_interface/ardrone_interface.c | 3 | ||||
-rw-r--r-- | apps/commander/commander.c | 48 | ||||
-rw-r--r-- | apps/multirotor_att_control/multirotor_att_control_main.c | 10 |
3 files changed, 23 insertions, 38 deletions
diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index f0bc0b1e7..8b4b5c400 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -140,9 +140,8 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin int speed = B115200; int uart; - /* open uart */ - printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); + //printf("[ardrone_interface] UART is %s, baud rate is%d\n",uart_name,speed); uart = open(uart_name, O_RDWR | O_NOCTTY); /* Try to set baud rate */ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 66a981a68..a1c2a15d2 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -312,8 +312,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) const uint64_t calibration_interval_us = 45 * 1000000; unsigned int calibration_counter = 0; - const int peak_samples = 500; - float mag_max[3] = {0, 0, 0}; float mag_min[3] = {0, 0, 0}; @@ -366,8 +364,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - //mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); - //break; + mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + break; } } @@ -389,20 +387,20 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) * offset = (max + min) / 2.0f */ -// printf("max 0: %f\n",mag_max[0]); -// printf("max 1: %f\n",mag_max[1]); -// printf("max 2: %f\n",mag_max[2]); -// printf("min 0: %f\n",mag_min[0]); -// printf("min 1: %f\n",mag_min[1]); -// printf("min 2: %f\n",mag_min[2]); + printf("max 0: %f\n",mag_max[0]); + printf("max 1: %f\n",mag_max[1]); + printf("max 2: %f\n",mag_max[2]); + printf("min 0: %f\n",mag_min[0]); + printf("min 1: %f\n",mag_min[1]); + printf("min 2: %f\n",mag_min[2]); mag_offset[0] = (mag_max[0] + mag_min[0]) / 2.0f; 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 0: %f\n",mag_offset[0]); -// printf("mag off 1: %f\n",mag_offset[1]); -// printf("mag off 2: %f\n",mag_offset[2]); + printf("mag off 0: %f\n",mag_offset[0]); + printf("mag off 1: %f\n",mag_offset[1]); + printf("mag off 2: %f\n",mag_offset[2]); /* announce and set new offset */ @@ -487,7 +485,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, retry"); return; } } @@ -534,9 +532,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "[commander] gyro calibration finished"); - // char offset_output[50]; - // sprintf(offset_output, "[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); + printf("[commander] gyro cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); close(sub_sensor_combined); } @@ -544,9 +540,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) void do_accel_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ - usleep(5000); - mavlink_log_info(mavlink_fd, "[commander] The system should be level and not moved"); + mavlink_log_info(mavlink_fd, "[commander] keep it level and still"); /* set to accel calibration mode */ status->flag_preflight_accel_calibration = true; state_machine_publish(status_pub, status, mavlink_fd); @@ -571,7 +566,6 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null)) warn("WARNING: failed to set scale / offsets for accel"); close(fd); - while (calibration_counter < calibration_count) { /* wait blocking for new data */ @@ -585,11 +579,10 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] gyro calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] acceleration calibration aborted"); return; } } - accel_offset[0] = accel_offset[0] / calibration_count; accel_offset[1] = accel_offset[1] / calibration_count; accel_offset[2] = accel_offset[2] / calibration_count; @@ -644,18 +637,11 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) if(save_ret != 0) { warn("WARNING: auto-save of params to EEPROM failed"); } - /* exit to gyro calibration mode */ status->flag_preflight_accel_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); - mavlink_log_info(mavlink_fd, "[commander] acceleration calibration finished"); - - // char offset_output[50]; - // sprintf(offset_output, "[commander] accel cal: x:%8.4f y:%8.4f z:%8.4f", (double)accel_offset[0], - // (double)accel_offset[1], (double)accel_offset[2]); - // mavlink_log_info(mavlink_fd, offset_output); - + printf("[commander] accel calibration: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0],(double)accel_offset[1], (double)accel_offset[2]); close(sub_sensor_combined); } @@ -823,7 +809,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* none found */ if (!handled) { //fprintf(stderr, "[commander] refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsupported calibration request"); + mavlink_log_critical(mavlink_fd, "[commander] CMD refusing unsup. calib. request"); result = MAV_RESULT_UNSUPPORTED; } } diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 8d648c194..b00b6bc0c 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -193,10 +193,10 @@ mc_thread_main(int argc, char *argv[]) /* ready, spawn pthread */ pthread_attr_t rate_control_attr; -// pthread_attr_init(&rate_control_attr); -// pthread_attr_setstacksize(&rate_control_attr, 2048); -// pthread_t rate_control_thread; -// pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); + pthread_attr_init(&rate_control_attr); + pthread_attr_setstacksize(&rate_control_attr, 2048); + pthread_t rate_control_thread; + pthread_create(&rate_control_thread, &rate_control_attr, rate_control_thread_main, NULL); while (!thread_should_exit) { @@ -310,7 +310,7 @@ mc_thread_main(int argc, char *argv[]) perf_print_counter(mc_loop_perf); perf_free(mc_loop_perf); - //pthread_join(rate_control_thread, NULL); + pthread_join(rate_control_thread, NULL); fflush(stdout); exit(0); |