OFFBOARD mode support in commander, mavlink, mc_att_control, WIP

5 files changed, 110 insertions, 74 deletions

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index dfcb1cc9f..7b69f2b76 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -411,42 +411,52 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			/* set main state */
 			transition_result_t main_res = TRANSITION_DENIED;
 
-			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-				/* use autopilot-specific mode */
-				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
-					/* MANUAL */
-					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
-
-				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
-					/* SEATBELT */
-					main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
-
-				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
-					/* EASY */
-					main_res = main_state_transition(status, MAIN_STATE_EASY);
-
-				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
-					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
-				}
+			if (status->rc_signal_lost) {
+				/* allow mode switching by command only if no RC signal */
+				if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
+					/* use autopilot-specific mode */
+					if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
 
-			} else {
-				/* use base mode */
-				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
-					/* AUTO */
-					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
+						/* SEATBELT */
+						main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
 
-				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
 						/* EASY */
 						main_res = main_state_transition(status, MAIN_STATE_EASY);
 
-					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
-						/* MANUAL */
-						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
+						/* AUTO */
+						main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+					} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
+						/* OFFBOARD */
+						main_res = main_state_transition(status, MAIN_STATE_OFFBOARD);
+					}
+
+				} else {
+					/* use base mode */
+					if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+						/* AUTO */
+						main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+					} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+						if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+							/* EASY */
+							main_res = main_state_transition(status, MAIN_STATE_EASY);
+
+						} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+							/* MANUAL */
+							main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+						}
 					}
 				}
+			} else {
+				mavlink_log_info(mavlink_fd, "RC signal is valid, ignoring set mode cmd");
 			}
+
 			if (main_res == TRANSITION_CHANGED)
 				ret = true;
 
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index b60a7e0b9..4843e72c3 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_SEATBELT,
 	PX4_CUSTOM_MAIN_MODE_EASY,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
+	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 4f8091716..70229558c 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -231,6 +231,8 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 		} else if (control_mode.nav_state == NAV_STATE_RTL) {
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_RTL;
 		}
+	} else if (v_status.main_state == MAIN_STATE_OFFBOARD) {
+		custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD;
 	}
 	*mavlink_custom_mode = custom_mode.data;
 
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 245ac024b..c4c6c0138 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -480,64 +480,73 @@ MulticopterAttitudeControl::task_main()
 			float yaw_sp_move_rate = 0.0f;
 			bool publish_att_sp = false;
 
-			/* define which input is the dominating control input */
-			if (_control_mode.flag_control_manual_enabled) {
-				/* manual input */
-				if (!_control_mode.flag_control_climb_rate_enabled) {
-					/* pass throttle directly if not in altitude control mode */
-					_att_sp.thrust = _manual.throttle;
-				}
+			if (_control_mode.flag_control_offboard_enabled) {
+				/* offboard control */
+				// TODO set _att_sp or _rates_sp here depending on offboard control mode
+				// TODO _control_mode.flag_control_XXX flags are all false, set it according to ofboard control mode
+				// but it's not very beautiful, need to think how to do it better
 
-				if (!_arming.armed) {
-					/* reset yaw setpoint when disarmed */
-					reset_yaw_sp = true;
-				}
+			} else {
+				/* onboard control */
+				/* define which input is the dominating control input */
+				if (_control_mode.flag_control_manual_enabled) {
+					/* manual input */
+					if (!_control_mode.flag_control_climb_rate_enabled) {
+						/* pass throttle directly if not in altitude control mode */
+						_att_sp.thrust = _manual.throttle;
+					}
 
-				if (_control_mode.flag_control_attitude_enabled) {
-					/* control attitude, update attitude setpoint depending on mode */
+					if (!_arming.armed) {
+						/* reset yaw setpoint when disarmed */
+						reset_yaw_sp = true;
+					}
 
-					if (_att_sp.thrust < 0.1f) {
-						// TODO
-						//if (_status.condition_landed) {
-						/* reset yaw setpoint if on ground */
-						//	reset_yaw_sp = true;
-						//}
-					} else {
-						if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
-							/* move yaw setpoint */
-							yaw_sp_move_rate = _manual.yaw;
-							_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
+					if (_control_mode.flag_control_attitude_enabled) {
+						/* control attitude, update attitude setpoint depending on mode */
+
+						if (_att_sp.thrust < 0.1f) {
+							// TODO
+							//if (_status.condition_landed) {
+							/* reset yaw setpoint if on ground */
+							//	reset_yaw_sp = true;
+							//}
+						} else {
+							if (_manual.yaw < -YAW_DEADZONE || YAW_DEADZONE < _manual.yaw) {
+								/* move yaw setpoint */
+								yaw_sp_move_rate = _manual.yaw;
+								_att_sp.yaw_body = _wrap_pi(_att_sp.yaw_body + yaw_sp_move_rate * dt);
+								_att_sp.R_valid = false;
+								publish_att_sp = true;
+							}
+						}
+
+						/* reset yaw setpint to current position if needed */
+						if (reset_yaw_sp) {
+							reset_yaw_sp = false;
+							_att_sp.yaw_body = _att.yaw;
 							_att_sp.R_valid = false;
 							publish_att_sp = true;
 						}
-					}
 
-					/* reset yaw setpint to current position if needed */
-					if (reset_yaw_sp) {
-						reset_yaw_sp = false;
-						_att_sp.yaw_body = _att.yaw;
-						_att_sp.R_valid = false;
-						publish_att_sp = true;
-					}
+						if (!_control_mode.flag_control_velocity_enabled) {
+							/* update attitude setpoint if not in position control mode */
+							_att_sp.roll_body = _manual.roll;
+							_att_sp.pitch_body = _manual.pitch;
+							_att_sp.R_valid = false;
+							publish_att_sp = true;
+						}
 
-					if (!_control_mode.flag_control_velocity_enabled) {
-						/* update attitude setpoint if not in position control mode */
-						_att_sp.roll_body = _manual.roll;
-						_att_sp.pitch_body = _manual.pitch;
-						_att_sp.R_valid = false;
-						publish_att_sp = true;
+					} else {
+						/* manual rate inputs (ACRO) */
+						// TODO
+						/* reset yaw setpoint after ACRO */
+						reset_yaw_sp = true;
 					}
 
 				} else {
-					/* manual rate inputs (ACRO) */
-					// TODO
-					/* reset yaw setpoint after ACRO */
+					/* reset yaw setpoint after non-manual control */
 					reset_yaw_sp = true;
 				}
-
-			} else {
-				/* reset yaw setpoint after non-manual control */
-				reset_yaw_sp = true;
 			}
 
 			if (_att_sp.R_valid) {
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ca5735509..7285e243e 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -1463,11 +1463,11 @@ Navigator::publish_control_mode()
 	_control_mode.flag_external_manual_override_ok = !_vstatus.is_rotary_wing;
 	_control_mode.flag_system_hil_enabled = _vstatus.hil_state == HIL_STATE_ON;
 
-	_control_mode.flag_control_offboard_enabled = false;
 	_control_mode.flag_control_flighttermination_enabled = false;
 
 	switch (_vstatus.main_state) {
 	case MAIN_STATE_MANUAL:
+		_control_mode.flag_control_offboard_enabled = false;
 		_control_mode.flag_control_manual_enabled = true;
 		_control_mode.flag_control_rates_enabled = _vstatus.is_rotary_wing;
 		_control_mode.flag_control_attitude_enabled = _vstatus.is_rotary_wing;
@@ -1478,6 +1478,7 @@ Navigator::publish_control_mode()
 		break;
 
 	case MAIN_STATE_SEATBELT:
+		_control_mode.flag_control_offboard_enabled = false;
 		_control_mode.flag_control_manual_enabled = true;
 		_control_mode.flag_control_rates_enabled = true;
 		_control_mode.flag_control_attitude_enabled = true;
@@ -1488,6 +1489,7 @@ Navigator::publish_control_mode()
 		break;
 
 	case MAIN_STATE_EASY:
+		_control_mode.flag_control_offboard_enabled = false;
 		_control_mode.flag_control_manual_enabled = true;
 		_control_mode.flag_control_rates_enabled = true;
 		_control_mode.flag_control_attitude_enabled = true;
@@ -1498,6 +1500,7 @@ Navigator::publish_control_mode()
 		break;
 
 	case MAIN_STATE_AUTO:
+		_control_mode.flag_control_offboard_enabled = false;
 		_control_mode.flag_control_manual_enabled = false;
 		if (myState == NAV_STATE_READY) {
 			/* disable all controllers, armed but idle */
@@ -1517,6 +1520,17 @@ Navigator::publish_control_mode()
 		}
 		break;
 
+	case MAIN_STATE_OFFBOARD:
+		_control_mode.flag_control_offboard_enabled = true;
+		_control_mode.flag_control_manual_enabled = false;
+		_control_mode.flag_control_rates_enabled = false;
+		_control_mode.flag_control_attitude_enabled = false;
+		_control_mode.flag_control_altitude_enabled = false;
+		_control_mode.flag_control_climb_rate_enabled = false;
+		_control_mode.flag_control_position_enabled = false;
+		_control_mode.flag_control_velocity_enabled = false;
+		break;
+
 	default:
 		break;
 	}