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

Merge branch 'master' of github.com:PX4/Firmware into px4io-i2c

This commit is contained in:
Lorenz Meier 2013-02-20 09:47:14 +01:00
parent 4cde275466 69d9265bc3
commit 16b0fa7fd4
1 changed files with 19 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

32
apps/commander/commander.c
View File

@ -421,7 +421,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
// mavlink_log_info(mavlink_fd, buf); // mavlink_log_info(mavlink_fd, buf);
// } // }
if (poll(fds, 1, 1000)) { int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
x[calibration_counter] = mag.x; x[calibration_counter] = mag.x;
@ -453,9 +455,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
calibration_counter++; calibration_counter++;
} else { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "mag cal canceled"); mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)");
break; break;
} }
} }
@ -545,7 +547,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status)
/* third beep by cal end routine */ /* third beep by cal end routine */
} else { } else {
mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN)"); mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
} }
/* disable calibration mode */ /* disable calibration mode */
@ -590,14 +592,16 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status)
/* wait blocking for new data */ /* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
if (poll(fds, 1, 1000)) { int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
gyro_offset[0] += raw.gyro_rad_s[0]; gyro_offset[0] += raw.gyro_rad_s[0];
gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[1] += raw.gyro_rad_s[1];
gyro_offset[2] += raw.gyro_rad_s[2]; gyro_offset[2] += raw.gyro_rad_s[2];
calibration_counter++; calibration_counter++;
} else { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
return; return;
@ -698,14 +702,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status)
/* wait blocking for new data */ /* wait blocking for new data */
struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } };
if (poll(fds, 1, 1000)) { int poll_ret = poll(fds, 1, 1000);
if (poll_ret) {
orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
accel_offset[0] += raw.accelerometer_m_s2[0]; accel_offset[0] += raw.accelerometer_m_s2[0];
accel_offset[1] += raw.accelerometer_m_s2[1]; accel_offset[1] += raw.accelerometer_m_s2[1];
accel_offset[2] += raw.accelerometer_m_s2[2]; accel_offset[2] += raw.accelerometer_m_s2[2];
calibration_counter++; calibration_counter++;
} else { } else if (poll_ret == 0) {
/* any poll failure for 1s is a reason to abort */ /* any poll failure for 1s is a reason to abort */
mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); mavlink_log_info(mavlink_fd, "acceleration calibration aborted");
return; return;
@ -1221,11 +1227,10 @@ int commander_main(int argc, char *argv[])
thread_should_exit = false; thread_should_exit = false;
daemon_task = task_spawn("commander", daemon_task = task_spawn("commander",
SCHED_DEFAULT, SCHED_DEFAULT,
SCHED_PRIORITY_MAX - 50, SCHED_PRIORITY_MAX - 40,
4000, 3000,
commander_thread_main, commander_thread_main,
(argv) ? (const char **)&argv[2] : (const char **)NULL); (argv) ? (const char **)&argv[2] : (const char **)NULL);
thread_running = true;
exit(0); exit(0);
} }
@ -1313,7 +1318,8 @@ int commander_thread_main(int argc, char *argv[])
memset(&home, 0, sizeof(home)); memset(&home, 0, sizeof(home));
if (stat_pub < 0) { if (stat_pub < 0) {
warnx("ERROR: orb_advertise for topic vehicle_status failed.\n"); warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n");
warnx("exiting.");
exit(ERROR); exit(ERROR);
} }
@ -1398,6 +1404,7 @@ int commander_thread_main(int argc, char *argv[])
/* now initialized */ /* now initialized */
commander_initialized = true; commander_initialized = true;
thread_running = true;
uint64_t start_time = hrt_absolute_time(); uint64_t start_time = hrt_absolute_time();
uint64_t failsave_ll_start_time = 0; uint64_t failsave_ll_start_time = 0;
@ -1405,7 +1412,6 @@ int commander_thread_main(int argc, char *argv[])
bool state_changed = true; bool state_changed = true;
bool param_init_forced = true; bool param_init_forced = true;
while (!thread_should_exit) { while (!thread_should_exit) {
/* Get current values */ /* Get current values */