From 34fb52d8bd3b2c6100c4c7adc7d789071cf3609c Mon Sep 17 00:00:00 2001
From: Matthias Grob <maetugr@gmail.com>
Date: Sun, 25 Nov 2018 11:11:51 +0000
Subject: [PATCH] commander: add orbit state handling
---
src/modules/commander/Commander.cpp | 20 ++++++++++++++++++-
.../commander/state_machine_helper.cpp | 16 ++++++++++++++-
2 files changed, 34 insertions(+), 2 deletions(-)
diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp
index 2e22328dff..bcf8b68972 100644
--- a/src/modules/commander/Commander.cpp
+++ b/src/modules/commander/Commander.cpp
@@ -1029,6 +1029,11 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
}
break;
+ case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
+ // Switch to orbit state and let the orbit task handle the command further
+ main_state_transition(*status_local, commander_state_s::MAIN_STATE_ORBIT, status_flags, &internal_state);
+ break;
+
case vehicle_command_s::VEHICLE_CMD_CUSTOM_0:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_1:
case vehicle_command_s::VEHICLE_CMD_CUSTOM_2:
@@ -1060,7 +1065,6 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_LOCATION:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_WPNEXT_OFFSET:
case vehicle_command_s::VEHICLE_CMD_DO_SET_ROI_NONE:
- case vehicle_command_s::VEHICLE_CMD_DO_ORBIT:
/* ignore commands that are handled by other parts of the system */
break;
@@ -3400,6 +3404,20 @@ set_control_mode()
break;
+ case vehicle_status_s::NAVIGATION_STATE_ORBIT:
+ control_mode.flag_control_manual_enabled = false;
+ control_mode.flag_control_auto_enabled = false;
+ control_mode.flag_control_rates_enabled = true;
+ control_mode.flag_control_attitude_enabled = true;
+ control_mode.flag_control_rattitude_enabled = false;
+ control_mode.flag_control_altitude_enabled = true;
+ control_mode.flag_control_climb_rate_enabled = true;
+ control_mode.flag_control_position_enabled = !status.in_transition_mode;
+ control_mode.flag_control_velocity_enabled = !status.in_transition_mode;
+ control_mode.flag_control_acceleration_enabled = false;
+ control_mode.flag_control_termination_enabled = false;
+ break;
+
default:
break;
}
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 89dc1c5293..d2d3bba5f3 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -294,8 +294,9 @@ main_state_transition(const vehicle_status_s &status, const main_state_t new_mai
break;
case commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET:
+ case commander_state_s::MAIN_STATE_ORBIT:
- /* FOLLOW only implemented in MC */
+ /* Follow and orbit only implemented for multicopter */
if (status.is_rotary_wing) {
ret = TRANSITION_CHANGED;
}
@@ -586,6 +587,19 @@ bool set_nav_state(vehicle_status_s *status, actuator_armed_s *armed, commander_
break;
+ case commander_state_s::MAIN_STATE_ORBIT:
+ /* require local position */
+ if (status->engine_failure) {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL;
+
+ } else if (is_armed && check_invalid_pos_nav_state(status, old_failsafe, mavlink_log_pub, status_flags, false, false)) {
+ // nothing to do - everything done in check_invalid_pos_nav_state
+
+ } else {
+ status->nav_state = vehicle_status_s::NAVIGATION_STATE_ORBIT;
+ }
+ break;
+
case commander_state_s::MAIN_STATE_AUTO_TAKEOFF:
/* require local position */
--
GitLab