fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-28 20:43:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Ignore measurement parts not projected to the plane

Browse Source
This commit is contained in:
Lorenz Meier
2012-10-16 00:02:53 +02:00
parent 40abed787c
commit 965bd35e2b
1 changed files with 21 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/commander/commander.c
+21 -17
View File
@@ -124,7 +124,6 @@ static bool thread_running = false; /**< Deamon status flag */
static int deamon_task; /**< Handle of deamon task / thread */ static int deamon_task; /**< Handle of deamon task / thread */
/* pthread loops */ /* pthread loops */
// static void *command_handling_loop(void *arg);
static void *orb_receive_loop(void *arg); static void *orb_receive_loop(void *arg);
__EXPORT int commander_main(int argc, char *argv[]); __EXPORT int commander_main(int argc, char *argv[]);
@@ -339,6 +338,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
axis_index++; axis_index++;
} }
if (!(axis_index < 3)) {
continue;
}
// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
// if ((axis_left / 1000) == 0 && axis_left > 0) { // if ((axis_left / 1000) == 0 && axis_left > 0) {
@@ -351,26 +354,27 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
/* get min/max values */ /* get min/max values */
if (raw.magnetometer_ga[0] < mag_min[0]) { /* ignore other axes */
mag_min[0] = raw.magnetometer_ga[0]; if (raw.magnetometer_ga[axis_index] < mag_min[axis_index]) {
mag_min[axis_index] = raw.magnetometer_ga[axis_index];
} }
else if (raw.magnetometer_ga[0] > mag_max[0]) { else if (raw.magnetometer_ga[axis_index] > mag_max[axis_index]) {
mag_max[0] = raw.magnetometer_ga[0]; mag_max[axis_index] = raw.magnetometer_ga[axis_index];
} }
if (raw.magnetometer_ga[1] < mag_min[1]) { // if (raw.magnetometer_ga[1] < mag_min[1]) {
mag_min[1] = raw.magnetometer_ga[1]; // mag_min[1] = raw.magnetometer_ga[1];
} // }
else if (raw.magnetometer_ga[1] > mag_max[1]) { // else if (raw.magnetometer_ga[1] > mag_max[1]) {
mag_max[1] = raw.magnetometer_ga[1]; // mag_max[1] = raw.magnetometer_ga[1];
} // }
if (raw.magnetometer_ga[2] < mag_min[2]) { // if (raw.magnetometer_ga[2] < mag_min[2]) {
mag_min[2] = raw.magnetometer_ga[2]; // mag_min[2] = raw.magnetometer_ga[2];
} // }
else if (raw.magnetometer_ga[2] > mag_max[2]) { // else if (raw.magnetometer_ga[2] > mag_max[2]) {
mag_max[2] = raw.magnetometer_ga[2]; // mag_max[2] = raw.magnetometer_ga[2];
} // }
calibration_counter++; calibration_counter++;
} else { } else {