diff options
author | Julian Oes <joes@student.ethz.ch> | 2013-02-22 15:53:07 -0800 |
---|---|---|
committer | Julian Oes <joes@student.ethz.ch> | 2013-02-22 15:53:07 -0800 |
commit | f731b6f4e5b009cf12d66c9ff2f5bbed47ca14af (patch) | |
tree | 5a74365c3100ec2ac46b2ade64c5a44164f987ec /apps/commander/commander.c | |
parent | 793b482e00013ea66bb1b0cdc0366bb720648938 (diff) | |
parent | be408451779dc53220ec94499a7acbe5ff3b8e7f (diff) | |
download | px4-firmware-f731b6f4e5b009cf12d66c9ff2f5bbed47ca14af.tar.gz px4-firmware-f731b6f4e5b009cf12d66c9ff2f5bbed47ca14af.tar.bz2 px4-firmware-f731b6f4e5b009cf12d66c9ff2f5bbed47ca14af.zip |
Merge remote-tracking branch 'upstream/px4io-i2c' into new_state_machine
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 32 |
1 files changed, 19 insertions, 13 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2784e77db..94e344da1 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -429,7 +429,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) // 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); x[calibration_counter] = mag.x; @@ -461,9 +463,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; - } else { + } else if (poll_ret == 0) { /* 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; } } @@ -553,7 +555,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* third beep by cal end routine */ } 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 */ @@ -598,14 +600,16 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) /* wait blocking for new data */ 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); gyro_offset[0] += raw.gyro_rad_s[0]; gyro_offset[1] += raw.gyro_rad_s[1]; gyro_offset[2] += raw.gyro_rad_s[2]; calibration_counter++; - } else { + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); return; @@ -706,14 +710,16 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) /* wait blocking for new data */ 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); accel_offset[0] += raw.accelerometer_m_s2[0]; accel_offset[1] += raw.accelerometer_m_s2[1]; accel_offset[2] += raw.accelerometer_m_s2[2]; calibration_counter++; - } else { + } else if (poll_ret == 0) { /* any poll failure for 1s is a reason to abort */ mavlink_log_info(mavlink_fd, "acceleration calibration aborted"); return; @@ -1253,11 +1259,10 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn("commander", SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 50, - 4000, + SCHED_PRIORITY_MAX - 40, + 3000, commander_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); - thread_running = true; exit(0); } @@ -1355,7 +1360,8 @@ int commander_thread_main(int argc, char *argv[]) memset(&home, 0, sizeof(home)); 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); } @@ -1445,6 +1451,7 @@ int commander_thread_main(int argc, char *argv[]) /* now initialized */ commander_initialized = true; + thread_running = true; uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; @@ -1452,7 +1459,6 @@ int commander_thread_main(int argc, char *argv[]) bool state_changed = true; bool param_init_forced = true; - while (!thread_should_exit) { /* Get current values */ |