fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-19 01:53:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'daregger_rate_control' of github.com:PX4/Firmware into calibration

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-19 00:39:06 +02:00
parent 97726fa679 5ec5754f26
commit d3ae83cb22
26 changed files with 1227 additions and 127 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+34 -23
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);
@@ -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, &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;