dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Fixed an abort condition, fixed value initialization, implemented naive three-step calibration

This commit is contained in:
Lorenz Meier 2012-10-17 15:09:28 +02:00
parent 23d294453b
commit 8b000b3317
1 changed files with 12 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
apps/commander/commander.c
View File

@ -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);
@ -333,13 +339,14 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
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();
@ -823,7 +830,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, &current_status);
ioctl(buzzer, TONE_SET_ALARM, 2);
mavlink_log_info(mavlink_fd, "[commander] CMD finished accel calibration");
do_state_update(status_pub, &current_status, mavlink_fd, SYSTEM_STATE_STANDBY);
result = MAV_RESULT_ACCEPTED;