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)