aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJulian Oes <joes@student.ethz.ch>2012-09-27 16:50:20 +0200
committerJulian Oes <joes@student.ethz.ch>2012-09-27 16:50:20 +0200
commit7f153098926bf977609c6efb53fa7cb5093564af (patch)
tree9c3ae6daab05346b1e06a2f2e0cb39e0e241cb66
parenta05c4d050473df87aacc95aadcd978d57f263cbc (diff)
downloadpx4-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.c3
-rw-r--r--apps/commander/commander.c48
-rw-r--r--apps/multirotor_att_control/multirotor_att_control_main.c10
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);