Fixed RTL command arming copter.usa_demo_2

4 files changed, 29 insertions, 47 deletions

diff --git a/src/modules/airdog/airdog.cpp b/src/modules/airdog/airdog.cpp
index 71b71129e..990ddb804 100644
--- a/src/modules/airdog/airdog.cpp
+++ b/src/modules/airdog/airdog.cpp
@@ -160,25 +160,27 @@ void cAirdog::cycle()
 		// handle_takeoff();
 
 		if(!hil) {
-			if (airdog_status.battery_remaining < bat_warning_level && battery_warning_count < 3 && airdog_status.battery_remaining!= 0)
-			{
-				mavlink_log_info(mavlink_fd, "remaining %d", airdog_status.battery_remaining);
-				battery_warning_count++;
-				ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
-				pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_SLOW);
-
-			} else if (airdog_status.battery_remaining < bat_critical_level && airdog_status.battery_remaining!= 0)
-			{
-				if (!rtl_triggered_from_battery && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_RTL && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_LAND)
-				{
-					rtl_triggered_from_battery = true;
-					// send_set_state(NAV_STATE_RTL, MOVE_NONE);
-					pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_FAST);
-				}
-			} else if (airdog_status.battery_remaining > bat_warning_level)
-			{
-				pi2c_ctrl->stop_blinking_led(I2C_LED_RED, true);
-			}
+			//TODO: [INE] rework!!!
+
+			// if (airdog_status.battery_remaining < bat_warning_level && battery_warning_count < 3 && airdog_status.battery_remaining!= 0)
+			// {
+			// 	mavlink_log_info(mavlink_fd, "remaining %d", airdog_status.battery_remaining);
+			// 	battery_warning_count++;
+			// 	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+			// 	pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_SLOW);
+
+			// } else if (airdog_status.battery_remaining < bat_critical_level && airdog_status.battery_remaining!= 0)
+			// {
+			// 	if (!rtl_triggered_from_battery && airdog_status.main_mode != PX4_CUSTOM_MAIN_MODE_RTL && airdog_status.sub_mode != PX4_CUSTOM_SUB_MODE_AUTO_LAND)
+			// 	{
+			// 		rtl_triggered_from_battery = true;
+			// 		// send_set_state(NAV_STATE_RTL, MOVE_NONE);
+			// 		pi2c_ctrl->start_blinking_led(I2C_LED_RED, BLINKING_RATE_FAST);
+			// 	}
+			// } else if (airdog_status.battery_remaining > bat_warning_level)
+			// {
+			// 	pi2c_ctrl->stop_blinking_led(I2C_LED_RED, true);
+			// }
 		}
 
 		if (airdog_status.timestamp > 0)
@@ -235,29 +237,6 @@ void cAirdog::send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_
 	}
 }
 
-void cAirdog::send_set_auto_mode(uint8_t base_mode, enum PX4_CUSTOM_SUB_MODE_AUTO custom_sub_mode_auto)
-{
-	struct vehicle_command_s cmd;
-	memset(&cmd, 0, sizeof(cmd));
-
-	/* fill command */
-	cmd.command = VEHICLE_CMD_DO_SET_MODE;
-	cmd.confirmation = false;
-	cmd.param1 = base_mode;
-	cmd.param2 = custom_sub_mode_auto;
-	cmd.source_system = vehicle_status.system_id;
-	cmd.source_component = vehicle_status.component_id;
-	// TODO add parameters AD_VEH_SYSID, AD_VEH_COMP to set target id
-	cmd.target_system = 1;
-	cmd.target_component = 50;
-
-	if (cmd_pub < 0) {
-		cmd_pub = orb_advertise(ORB_ID(vehicle_command), &cmd);
-	} else {
-		orb_publish(ORB_ID(vehicle_command), cmd_pub, &cmd);
-	}
-}
-
 void cAirdog::send_command(enum REMOTE_CMD command)
 {
 	struct vehicle_command_s cmd;
@@ -382,10 +361,10 @@ bool cAirdog::button_clicked_i2c(uint8_t button, bool long_press)
                 send_command(REMOTE_CMD_DOWN);
             } else if (current_button_state == BUTTON_STATE_CHOOSE_FUNCTION){
 
-                uint8_t base_mode = MAV_MODE_FLAG_SAFETY_ARMED | MAV_MODE_FLAG_AUTO_ENABLED;
+                uint8_t base_mode = MAV_MODE_FLAG_SAFETY_ARMED | MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
                 if (hil) base_mode |= MAV_MODE_FLAG_HIL_ENABLED;
 
-                send_set_auto_mode(base_mode, PX4_CUSTOM_SUB_MODE_AUTO_RTL);
+                send_set_mode(base_mode, PX4_CUSTOM_MAIN_MODE_RTL);
                 set_current_button_state(BUTTON_STATE_DEFAULT);
 
                 set_current_button_state(BUTTON_STATE_DEFAULT);
diff --git a/src/modules/airdog/airdog.h b/src/modules/airdog/airdog.h
index 67802214a..12d7642ec 100644
--- a/src/modules/airdog/airdog.h
+++ b/src/modules/airdog/airdog.h
@@ -89,7 +89,6 @@ private:
 
 
 	void send_set_mode(uint8_t base_mode, enum PX4_CUSTOM_MAIN_MODE custom_main_mode, int mode_args = 0);
-	void send_set_auto_mode(uint8_t base_mode, enum PX4_CUSTOM_SUB_MODE_AUTO custom_sub_mode_auto);
 	void send_command(enum REMOTE_CMD command);
 	// void send_set_state(enum NAV_STATE state, enum AUTO_MOVE_DIRECTION direction);
 	// void send_set_move(enum AUTO_MOVE_DIRECTION direction);
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 72793f270..3db84c815 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -499,7 +499,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 
 					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_FOLLOW) {
 						/* FOLLOW */
-						main_ret = main_state_transition(status_local, MAIN_STATE_FOLLOW, mavlink_fd);
+						main_ret = main_state_transition(status_local, MAIN_STATE_FOLLOW, mavlink_fd);					
+					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_RTL) {
+						/* AUTO RTL */
+						//This mode should be processed in LOITER submodes
+						main_ret = TRANSITION_DENIED;
 					}
 
 				} else {
diff --git a/src/modules/navigator/loiter.cpp b/src/modules/navigator/loiter.cpp
index f023f2f96..c3bcaf0e2 100644
--- a/src/modules/navigator/loiter.cpp
+++ b/src/modules/navigator/loiter.cpp
@@ -205,7 +205,7 @@ Loiter::execute_command_in_aim_and_shoot(vehicle_command_s cmd){
 		//uint8_t base_mode = (uint8_t)cmd.param1;
 		uint8_t main_mode = (uint8_t)cmd.param2;
 
-		if (main_mode == PX4_CUSTOM_SUB_MODE_AUTO_RTL) {
+		if (main_mode == PX4_CUSTOM_MAIN_MODE_RTL) {
 
 			commander_request_s *commander_request = _navigator->get_commander_request();
 			commander_request->request_type = V_MAIN_STATE_CHANGE;