diff options
author | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 16:01:42 +0200 |
---|---|---|
committer | tnaegeli <naegelit@student.ethz.ch> | 2012-10-04 16:01:42 +0200 |
commit | 8dfa66cb9710f1f5f8baddb6d0b542787af44f15 (patch) | |
tree | 63d99d8861187b891274172a0f95a1fd816c48e9 /apps/commander/commander.c | |
parent | b9de72a8c9e97165c190020adf2d99849daf5f3a (diff) | |
parent | 2a06b66845542b05e3cad3d21099e33adc213227 (diff) | |
download | px4-firmware-8dfa66cb9710f1f5f8baddb6d0b542787af44f15.tar.gz px4-firmware-8dfa66cb9710f1f5f8baddb6d0b542787af44f15.tar.bz2 px4-firmware-8dfa66cb9710f1f5f8baddb6d0b542787af44f15.zip |
Merge branch 'master' of https://github.com/PX4/Firmware
Conflicts:
apps/commander/commander.c
apps/multirotor_att_control/multirotor_att_control_main.c
apps/multirotor_att_control/multirotor_rate_control.c
Diffstat (limited to 'apps/commander/commander.c')
-rw-r--r-- | apps/commander/commander.c | 19 |
1 files changed, 7 insertions, 12 deletions
diff --git a/apps/commander/commander.c b/apps/commander/commander.c index f29cef165..c569a6aa4 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -293,7 +293,7 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) struct sensor_combined_s raw; /* 30 seconds */ - const uint64_t calibration_interval_us = 45 * 1000000; + int calibration_interval_ms = 30 * 1000; unsigned int calibration_counter = 0; float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; @@ -312,10 +312,10 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) warn("WARNING: failed to set scale / offsets for mag"); close(fd); - mavlink_log_info(mavlink_fd, "[commander] Please rotate around all axes."); + mavlink_log_info(mavlink_fd, "[commander] Please rotate around X"); uint64_t calibration_start = hrt_absolute_time(); - while ((hrt_absolute_time() - calibration_start) < calibration_interval_us) { + while ((hrt_absolute_time() - calibration_start)/1000 < calibration_interval_ms) { /* wait blocking for new data */ struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; @@ -348,11 +348,13 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) calibration_counter++; } else { /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "[commander] mag calibration aborted, please retry."); + mavlink_log_info(mavlink_fd, "[commander] mag cal canceled"); break; } } + mavlink_log_info(mavlink_fd, "[commander] mag calibration done"); + /* disable calibration mode */ status->flag_preflight_mag_calibration = false; state_machine_publish(status_pub, status, mavlink_fd); @@ -1307,19 +1309,12 @@ int commander_thread_main(int argc, char *argv[]) //printf("RC: y:%i/t:%i s:%i chans: %i\n", rc_yaw_scale, rc_throttle_scale, mode_switch_rc_value, rc.chan_count); if (sp_man.override_mode_switch > STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); } else if (sp_man.override_mode_switch < -STICK_ON_OFF_LIMIT) { - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - //update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); + update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); } else { - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = true; update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } |