1 files changed, 128 insertions, 83 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5eeb018fd..01b7b84d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -5,6 +5,7 @@
  *           Lorenz Meier <lm@inf.ethz.ch>
  *           Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -198,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
-void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
@@ -369,7 +370,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
 			if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) {
-				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				if (safety->safety_switch_available && !safety->safety_off) {
+					print_reject_arm("NOT ARMING: Press safety switch first.");
+					arming_res = TRANSITION_DENIED;
+
+				} else {
+					arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+				}
 
 				if (arming_res == TRANSITION_CHANGED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by command");
@@ -463,8 +470,26 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 
 	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
-		// XXX implement later
-		result = VEHICLE_CMD_RESULT_DENIED;
+	{
+		transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+		if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+			if (safety->safety_switch_available && !safety->safety_off) {
+				print_reject_arm("NOT ARMING: Press safety switch first.");
+				arming_res = TRANSITION_DENIED;
+
+			} else {
+				arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed);
+			}
+
+			if (arming_res == TRANSITION_CHANGED) {
+				mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+				result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else {
+				mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+	}
 		break;
 
 	default:
@@ -561,7 +586,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* flags for control apps */
 	struct vehicle_control_mode_s control_mode;
-	
+
 
 	/* Initialize all flags to false */
 	memset(&control_mode, 0, sizeof(control_mode));
@@ -644,7 +669,6 @@ int commander_thread_main(int argc, char *argv[])
 	bool critical_battery_voltage_actions_done = false;
 
 	uint64_t last_idle_time = 0;
-
 	uint64_t start_time = 0;
 
 	bool status_changed = true;
@@ -722,7 +746,7 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
-	toggle_status_leds(&status, &armed, true);
+	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
 	commander_initialized = true;
@@ -800,16 +824,6 @@ int commander_thread_main(int argc, char *argv[])
 
 		check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed);
 
-		orb_check(cmd_sub, &updated);
-
-		if (updated) {
-			/* got command */
-			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
-
-			/* handle it */
-			handle_command(&status, &safety, &control_mode, &cmd, &armed);
-		}
-
 		/* update safety topic */
 		orb_check(safety_sub, &updated);
 
@@ -944,11 +958,9 @@ int commander_thread_main(int argc, char *argv[])
 				battery_tune_played = false;
 
 				if (armed.armed) {
-					// XXX not sure what should happen when voltage is low in flight
-					//arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
 				} else {
-					// XXX should we still allow to arm with critical battery?
-					//arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
+					arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
 
 				status_changed = true;
@@ -1083,10 +1095,18 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 				if (status.arming_state == ARMING_STATE_STANDBY &&
-				    status.main_state == MAIN_STATE_MANUAL &&
 				    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
-						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						if (safety.safety_switch_available && !safety.safety_off) {
+							print_reject_arm("NOT ARMING: Press safety switch first.");
+
+						} else if (status.main_state != MAIN_STATE_MANUAL) {
+							print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+
+						} else {
+							res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						}
+
 						stick_on_counter = 0;
 
 					} else {
@@ -1106,15 +1126,8 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 				} else if (res == TRANSITION_DENIED) {
-					/* DENIED here indicates safety switch not pressed */
-
-					if (!(!safety.safety_switch_available || safety.safety_off)) {
-						print_reject_arm("NOT ARMING: Press safety switch first.");
-
-					} else {
-						warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-						mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					}
+					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 				/* fill current_status according to mode switches */
@@ -1142,6 +1155,18 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+
+		/* handle commands last, as the system needs to be updated to handle them */
+		orb_check(cmd_sub, &updated);
+
+		if (updated) {
+			/* got command */
+			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
+
+			/* handle it */
+			handle_command(&status, &safety, &control_mode, &cmd, &armed);
+		}
+
 		/* evaluate the navigation state machine */
 		transition_result_t res = check_navigation_state_machine(&status, &control_mode, &local_position);
 
@@ -1159,9 +1184,6 @@ int commander_thread_main(int argc, char *argv[])
 		if (arming_state_changed || main_state_changed || navigation_state_changed) {
 			mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state);
 			status_changed = true;
-
-		} else {
-			status_changed = false;
 		}
 
 		hrt_abstime t1 = hrt_absolute_time();
@@ -1221,13 +1243,26 @@ int commander_thread_main(int argc, char *argv[])
 		fflush(stdout);
 		counter++;
 
-		toggle_status_leds(&status, &armed, arming_state_changed || status_changed);
+		int blink_state = blink_msg_state();
+		if (blink_state > 0) {
+			/* blinking LED message, don't touch LEDs */
+			if (blink_state == 2) {
+				/* blinking LED message completed, restore normal state */
+				control_status_leds(&status, &armed, true);
+			}
+		} else {
+			/* normal state */
+			control_status_leds(&status, &armed, status_changed);
+		}
+
+		status_changed = false;
 
 		usleep(COMMANDER_MONITORING_INTERVAL);
 	}
 
 	/* wait for threads to complete */
 	ret = pthread_join(commander_low_prio_thread, NULL);
+
 	if (ret) {
 		warn("join failed", ret);
 	}
@@ -1268,8 +1303,48 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
 {
+	/* driving rgbled */
+	if (changed) {
+		bool set_normal_color = false;
+		/* set mode */
+		if (status->arming_state == ARMING_STATE_ARMED) {
+			rgbled_set_mode(RGBLED_MODE_ON);
+			set_normal_color = true;
+
+		} else if (status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+			rgbled_set_color(RGBLED_COLOR_RED);
+
+		} else if (status->arming_state == ARMING_STATE_STANDBY) {
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
+			set_normal_color = true;
+
+		} else {	// STANDBY_ERROR and other states
+			rgbled_set_mode(RGBLED_MODE_BLINK_NORMAL);
+			rgbled_set_color(RGBLED_COLOR_RED);
+		}
+
+		if (set_normal_color) {
+			/* set color */
+			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+					rgbled_set_color(RGBLED_COLOR_AMBER);
+				}
+				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
+
+			} else {
+				if (status->condition_local_position_valid) {
+					rgbled_set_color(RGBLED_COLOR_GREEN);
+
+				} else {
+					rgbled_set_color(RGBLED_COLOR_BLUE);
+				}
+			}
+		}
+	}
+
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
@@ -1290,46 +1365,6 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 #endif
 
-	if (changed) {
-		/* XXX TODO blink fast when armed and serious error occurs */
-
-		if (armed->armed) {
-			rgbled_set_mode(RGBLED_MODE_ON);
-		} else if (armed->ready_to_arm) {
-			rgbled_set_mode(RGBLED_MODE_BREATHE);
-		} else {
-			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-		}
-	}
-
-	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
-		switch (status->battery_warning) {
-			case VEHICLE_BATTERY_WARNING_LOW:
-				rgbled_set_color(RGBLED_COLOR_YELLOW);
-				break;
-			case VEHICLE_BATTERY_WARNING_CRITICAL:
-				rgbled_set_color(RGBLED_COLOR_AMBER);
-				break;
-			default:
-				break;
-		}
-	} else {
-		switch (status->main_state) {
-			case MAIN_STATE_MANUAL:
-				rgbled_set_color(RGBLED_COLOR_WHITE);
-				break;
-			case MAIN_STATE_SEATBELT:
-			case MAIN_STATE_EASY:
-				rgbled_set_color(RGBLED_COLOR_GREEN);
-				break;
-			case MAIN_STATE_AUTO:
-				rgbled_set_color(RGBLED_COLOR_BLUE);
-				break;
-			default:
-				break;
-		}
-	}
-
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
 	if (status->load > 0.95f) {
 		if (leds_counter % 2 == 0)
@@ -1497,16 +1532,18 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 					return TRANSITION_NOT_CHANGED;
 				}
 			}
+
 			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
-				status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
+			    status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
+			    status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
+			    status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
 				/* possibly on ground, switch to TAKEOFF if needed */
 				if (local_pos->z > -takeoff_alt || status->condition_landed) {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
 					return res;
 				}
 			}
+
 			/* switch to AUTO mode */
 			if (status->rc_signal_found_once && !status->rc_signal_lost) {
 				/* act depending on switches when manual control enabled */
@@ -1524,18 +1561,20 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 					}
 				}
+
 			} else {
 				/* switch to MISSION when no RC control and first time in some AUTO mode  */
 				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-					status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+				    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+				    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+				    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
 					res = TRANSITION_NOT_CHANGED;
 
 				} else {
 					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 				}
 			}
+
 		} else {
 			/* disarmed, always switch to AUTO_READY */
 			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1762,35 +1801,41 @@ void *commander_low_prio_loop(void *arg)
 
 				if (((int)(cmd.param1)) == 0) {
 					int ret = param_load_default();
+
 					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
 							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 
 				} else if (((int)(cmd.param1)) == 1) {
 					int ret = param_save_default();
+
 					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
 							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 				}