1 files changed, 154 insertions, 8 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 6ef7a3144..b7347c81c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -83,6 +83,8 @@
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
 #include <uORB/topics/telemetry_status.h>
+ #include <uORB/topics/commander_request.h>
+
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -173,6 +175,8 @@ static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
 static struct offboard_control_setpoint_s sp_offboard;
 
+int mode_switch_state = -1;
+
 /* tasks waiting for low prio thread */
 typedef enum {
 	LOW_PRIO_TASK_NONE = 0,
@@ -424,6 +428,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			uint8_t base_mode = (uint8_t)cmd->param1;
 			uint8_t custom_main_mode = (uint8_t)cmd->param2;
 
+			int bm = base_mode;
+			int cm = custom_main_mode;
+
+			mavlink_log_info(mavlink_fd,"Command base: %d, custom mode: %d vb", bm, cm);
+
 			transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
 
 			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
@@ -450,8 +459,8 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 					main_ret = main_state_transition(status_local, MAIN_STATE_POSCTL);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-					/* AUTO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+					//* AUTO */
+					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
 
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
 					/* ACRO */
@@ -470,7 +479,7 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 				/* use base mode */
 				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
 					/* AUTO */
-					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
+					//main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_MISSION);
 
 				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
 					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
@@ -592,6 +601,27 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
+	/* Airleash commands */
+	case VEHICLE_CMD_NAV_REMOTE_CMD: {
+			transition_result_t main_ret = TRANSITION_NOT_CHANGED;
+
+			/*
+			if (cmd->param1 == REMOTE_CMD_PLAY_PAUSE) {
+				if(status_local->main_state == MAIN_STATE_AUTO_LOITER) {
+					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_ABS_FOLLOW);
+				} else {
+					main_ret = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+				}
+			}
+			*/
+			if (main_ret != TRANSITION_DENIED){
+				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
+			} else {
+				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+			}
+		}
+		break;
+
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
 
@@ -716,6 +746,18 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_ef_current2throttle_thres = param_find("COM_EF_C2T");
 	param_t _param_ef_time_thres = param_find("COM_EF_TIME");
 
+	float battery_warning_level;
+	float battery_critical_level;
+	float battery_flat_level;
+
+	param_t _param_battery_warning_level = param_find("BAT_WATN_LVL");
+	param_t _param_battery_critical_level = param_find("BAT_CRIT_LVL");
+	param_t _param_battery_flat_level = param_find("BAT_FLAT_LVL");
+
+	param_get(_param_battery_warning_level, &battery_warning_level);
+	param_get(_param_battery_critical_level, &battery_critical_level);
+	param_get(_param_battery_flat_level, &battery_flat_level);
+
 	/* welcome user */
 	warnx("starting");
 
@@ -747,6 +789,7 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_AUTO_ABS_FOLLOW]    = "AUTO_ABS_FOLLOW";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
@@ -877,6 +920,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool low_battery_voltage_actions_done = false;
 	bool critical_battery_voltage_actions_done = false;
+	bool flat_battery_voltage_actions_done = false;
 
 	hrt_abstime last_idle_time = 0;
 	hrt_abstime start_time = 0;
@@ -997,6 +1041,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct actuator_controls_s actuator_controls;
 	memset(&actuator_controls, 0, sizeof(actuator_controls));
 
+	/* Subscribe to commander requests - requests made for commander to process */
+	int commander_request_sub = orb_subscribe(ORB_ID(commander_request));
+	struct commander_request_s commander_request;
+	memset(&commander_request, 0, sizeof(commander_request));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -1399,13 +1448,17 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
+		if (status.condition_battery_voltage_valid && status.battery_remaining < battery_warning_level && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
-			status_changed = true;
 
-		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+			if (! control_mode.flag_control_manual_enabled) {
+				if (main_state_transition(&status, MAIN_STATE_AUTO_RTL)) {
+					status_changed = true;
+				}
+			}
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < battery_critical_level && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@@ -1426,6 +1479,10 @@ int commander_thread_main(int argc, char *argv[])
 				}
 			}
 			status_changed = true;
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < battery_flat_level && !flat_battery_voltage_actions_done){
+			flat_battery_voltage_actions_done = true;
+			status.battery_warning = VEHICLE_BATTERY_WARNING_FLAT;
+			// TODO urgent land
 		}
 
 		/* End battery voltage check */
@@ -1665,6 +1722,39 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
+		/* Handle commander requests here */
+		orb_check(commander_request_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(commander_request), commander_request_sub, &commander_request);
+
+			switch(commander_request.request_type) {
+			case V_MAIN_STATE_CHANGE:
+			{
+
+				if (main_state_transition(&status, commander_request.main_state)) {
+					status_changed = true;
+				}
+
+				break;
+			}
+			case V_DISARM:
+			{
+				arm_disarm(false, mavlink_fd, "Commander request.");
+				break;
+			}
+			case V_NAVIGATION_STATE_CHANGE:
+				break;
+			default:
+				break;
+
+			}
+
+			mavlink_log_info(mavlink_fd, "Commnander request processed\n");
+
+
+		}
+
 		/* Check engine failure
 		 * only for fixed wing for now
 		 */
@@ -1795,8 +1885,7 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* now set navigation state according to failsafe and main state */
 		bool nav_state_changed = set_nav_state(&status, (bool)datalink_loss_enabled,
-						       mission_result.finished,
-						       mission_result.stay_in_failsafe);
+						       mission_result.finished, mission_result.stay_in_failsafe, mavlink_fd);
 
 		// TODO handle mode changes by commands
 		if (main_state_changed) {
@@ -2021,10 +2110,16 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	/* offboard switched off or denied, check main mode switch */
 	switch (sp_man->mode_switch) {
 	case SWITCH_POS_NONE:
+		
+        mode_switch_state = SWITCH_POS_NONE; 
+		
 		res = TRANSITION_NOT_CHANGED;
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL
+		
+        mode_switch_state = SWITCH_POS_OFF; 
+
 		if (sp_man->acro_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status_local, MAIN_STATE_ACRO);
 
@@ -2035,6 +2130,9 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		break;
 
 	case SWITCH_POS_MIDDLE:		// ASSIST
+
+        mode_switch_state = SWITCH_POS_MIDDLE;
+
 		if (sp_man->follow_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status_local, MAIN_STATE_FOLLOW);
 
@@ -2074,6 +2172,26 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		break;
 
 	case SWITCH_POS_ON:			// AUTO
+		
+		// Switch to loter only when switch state have changed
+        if (mode_switch_state != SWITCH_POS_ON) {
+
+            mode_switch_state = SWITCH_POS_ON;
+
+            res = main_state_transition(status_local, MAIN_STATE_AUTO_LOITER);
+
+            if (res != TRANSITION_DENIED) {
+                break;  // changed successfully or already in this state
+            }
+
+            print_reject_mode(status_local, "AUTO_LOITER");
+
+        } else {
+            res = TRANSITION_NOT_CHANGED; 
+            break;
+        }
+
+		/*
 		if (sp_man->return_switch == SWITCH_POS_ON) {
 			res = main_state_transition(status_local, MAIN_STATE_AUTO_RTL);
 
@@ -2133,6 +2251,9 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 		// fallback to MANUAL
 		res = main_state_transition(status_local, MAIN_STATE_MANUAL);
 		// TRANSITION_DENIED is not possible here
+
+
+		*/
 		break;
 
 	default:
@@ -2272,6 +2393,16 @@ set_control_mode()
 
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		control_mode.flag_control_point_to_target = true;
 	case NAVIGATION_STATE_AUTO_RTL:
 	case NAVIGATION_STATE_AUTO_RTGS:
 	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
@@ -2298,6 +2429,20 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_AUTO_ABS_FOLLOW:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = true;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_position_enabled = true;
+		control_mode.flag_control_velocity_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		control_mode.flag_control_follow_target = true;
+		control_mode.flag_control_point_to_target = true;
+		break;
+
 	case NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
@@ -2428,6 +2573,7 @@ void *commander_low_prio_loop(void *arg)
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
 		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
+		    cmd.command == VEHICLE_CMD_NAV_REMOTE_CMD ||
 		    cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
 			continue;
 		}