diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-01 19:13:13 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-01 19:13:13 +0100 |
commit | 020e7dcae36584deffb5b7e3bb453bb9950a1966 (patch) | |
tree | d43af7cab236e9cfd503a15094917d460fe3a49e /src/modules/commander/commander.cpp | |
parent | dd7c198268836b06119a6127601a93cf152f288e (diff) | |
parent | 713f35e04edc7f79f069ced23023345177fe6794 (diff) | |
download | px4-firmware-020e7dcae36584deffb5b7e3bb453bb9950a1966.tar.gz px4-firmware-020e7dcae36584deffb5b7e3bb453bb9950a1966.tar.bz2 px4-firmware-020e7dcae36584deffb5b7e3bb453bb9950a1966.zip |
Merged master into beta
Diffstat (limited to 'src/modules/commander/commander.cpp')
-rw-r--r-- | src/modules/commander/commander.cpp | 50 |
1 files changed, 41 insertions, 9 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 40117b7a1..a21611beb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -210,7 +210,7 @@ transition_result_t set_main_state_rc(struct vehicle_status_s *status); void set_control_mode(); -void print_reject_mode(const char *msg); +void print_reject_mode(struct vehicle_status_s *current_status, const char *msg); void print_reject_arm(const char *msg); @@ -657,6 +657,8 @@ int commander_thread_main(int argc, char *argv[]) /* vehicle status topic */ memset(&status, 0, sizeof(status)); status.condition_landed = true; // initialize to safe value + // We want to accept RC inputs as default + status.rc_input_blocked = false; status.main_state = MAIN_STATE_MANUAL; status.set_nav_state = NAV_STATE_NONE; status.set_nav_state_timestamp = 0; @@ -1104,7 +1106,7 @@ int commander_thread_main(int argc, char *argv[]) } /* start RC input check */ - if (sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { + if (!status.rc_input_blocked && sp_man.timestamp != 0 && hrt_absolute_time() < sp_man.timestamp + RC_TIMEOUT) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; @@ -1542,7 +1544,7 @@ set_main_state_rc(struct vehicle_status_s *status) break; // changed successfully or already in this state // else fallback to SEATBELT - print_reject_mode("EASY"); + print_reject_mode(status, "EASY"); } res = main_state_transition(status, MAIN_STATE_SEATBELT); @@ -1551,7 +1553,7 @@ set_main_state_rc(struct vehicle_status_s *status) break; // changed successfully or already in this mode if (status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages - print_reject_mode("SEATBELT"); + print_reject_mode(status, "SEATBELT"); // else fallback to MANUAL res = main_state_transition(status, MAIN_STATE_MANUAL); @@ -1565,7 +1567,7 @@ set_main_state_rc(struct vehicle_status_s *status) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - print_reject_mode("AUTO"); + print_reject_mode(status, "AUTO"); res = main_state_transition(status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) @@ -1584,6 +1586,7 @@ set_main_state_rc(struct vehicle_status_s *status) } void + set_control_mode() { /* set vehicle_control_mode according to main state and failsafe state */ @@ -1680,16 +1683,25 @@ set_control_mode() } void -print_reject_mode(const char *msg) +print_reject_mode(struct vehicle_status_s *status, const char *msg) { hrt_abstime t = hrt_absolute_time(); if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { last_print_mode_reject_time = t; char s[80]; - sprintf(s, "#audio: warning: reject %s", msg); + sprintf(s, "#audio: REJECT %s", msg); mavlink_log_critical(mavlink_fd, s); - tune_negative(); + + // only buzz if armed, because else we're driving people nuts indoors + // they really need to look at the leds as well. + if (status->arming_state == ARMING_STATE_ARMED) { + tune_negative(); + } else { + + // Always show the led indication + led_negative(); + } } } @@ -1837,7 +1849,15 @@ void *commander_low_prio_loop(void *arg) } else if ((int)(cmd.param4) == 1) { /* RC calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); - calib_ret = do_rc_calibration(mavlink_fd); + /* disable RC control input completely */ + status.rc_input_blocked = true; + calib_ret = OK; + mavlink_log_info(mavlink_fd, "CAL: Disabling RC IN"); + + } else if ((int)(cmd.param4) == 2) { + /* RC trim calibration */ + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + calib_ret = do_trim_calibration(mavlink_fd); } else if ((int)(cmd.param5) == 1) { /* accelerometer calibration */ @@ -1848,6 +1868,18 @@ void *commander_low_prio_loop(void *arg) /* airspeed calibration */ answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); calib_ret = do_airspeed_calibration(mavlink_fd); + } else if ((int)(cmd.param4) == 0) { + /* RC calibration ended - have we been in one worth confirming? */ + if (status.rc_input_blocked) { + answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED); + /* enable RC control input */ + status.rc_input_blocked = false; + mavlink_log_info(mavlink_fd, "CAL: Re-enabling RC IN"); + } + + /* this always succeeds */ + calib_ret = OK; + } if (calib_ret == OK) |