Completely implemented offboard control

5 files changed, 130 insertions, 29 deletions

diff --git a/apps/uORB/objects_common.cpp b/apps/uORB/objects_common.cpp
index be85a345a..f211648a9 100644
--- a/apps/uORB/objects_common.cpp
+++ b/apps/uORB/objects_common.cpp
@@ -59,7 +59,6 @@ ORB_DEFINE(output_pwm, struct pwm_output_values);
 #include <drivers/drv_rc_input.h>
 ORB_DEFINE(input_rc, struct rc_input_values);
 
-// XXX need to check wether these should be here
 #include "topics/vehicle_attitude.h"
 ORB_DEFINE(vehicle_attitude, struct vehicle_attitude_s);
 
@@ -99,6 +98,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);
 
+#include "topics/offboard_control_setpoint.h"
+ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s);
+
 #include "topics/optical_flow.h"
 ORB_DEFINE(optical_flow, struct optical_flow_s);
 
diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h
index b01c7438d..a7f5be708 100644
--- a/apps/uORB/topics/manual_control_setpoint.h
+++ b/apps/uORB/topics/manual_control_setpoint.h
@@ -1,21 +1,21 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
  * are met:
  *
  * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
+ *		notice, this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
+ *		notice, this list of conditions and the following disclaimer in
+ *		the documentation and/or other materials provided with the
+ *		distribution.
  * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
+ *		used to endorse or promote products derived from this software
+ *		without specific prior written permission.
  *
  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
@@ -57,31 +57,27 @@
  */
 enum MANUAL_CONTROL_MODE
 {
-  DIRECT = 0,
-  ROLLPOS_PITCHPOS_YAWRATE_THROTTLE = 1,
-	ROLLRATE_PITCHRATE_YAWRATE_THROTTLE = 2,
-  ROLLPOS_PITCHPOS_YAWPOS_THROTTLE = 3
+	MANUAL_CONTROL_MODE_DIRECT = 0,
+	MANUAL_CONTROL_MODE_ATT_YAW_RATE = 1,
+	MANUAL_CONTROL_MODE_ATT_YAW_POS = 2,
+	MANUAL_CONTROL_MODE_MULTIROTOR_SIMPLE = 3 /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
 struct manual_control_setpoint_s {
+	uint64_t timestamp;
 
-  enum MANUAL_CONTROL_MODE mode;     /**< The current control inputs mode */
-  float roll;                   /**< roll / roll rate input */
-  float pitch;
-  float yaw;
-  float throttle;
+	enum MANUAL_CONTROL_MODE mode;		 /**< The current control inputs mode */
+	float roll;			 /**< ailerons roll / roll rate input */
+	float pitch;     /**< elevator / pitch / pitch rate */
+	float yaw;       /**< rudder / yaw rate / yaw */
+	float throttle;  /**< throttle / collective thrust / altitude */
 
-  float override_mode_switch;
+	float override_mode_switch;
 
-  float ailerons;
-  float elevator;
-  float rudder;
-  float flaps;
-
-  float aux1_cam_pan;
-  float aux2_cam_tilt;
-  float aux3_cam_zoom;
-  float aux4_cam_roll;
+	float aux1_cam_pan_flaps;
+	float aux2_cam_tilt;
+	float aux3_cam_zoom;
+	float aux4_cam_roll;
 
 }; /**< manual control inputs */
 
diff --git a/apps/uORB/topics/offboard_control_setpoint.h b/apps/uORB/topics/offboard_control_setpoint.h
new file mode 100644
index 000000000..2e895c59c
--- /dev/null
+++ b/apps/uORB/topics/offboard_control_setpoint.h
@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *	 Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
+ *	 Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *		notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *		notice, this list of conditions and the following disclaimer in
+ *		the documentation and/or other materials provided with the
+ *		distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *		used to endorse or promote products derived from this software
+ *		without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file offboard_control_setpoint.h
+ * Definition of the manual_control_setpoint uORB topic.
+ */
+
+#ifndef TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
+#define TOPIC_OFFBOARD_CONTROL_SETPOINT_H_
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Off-board control inputs.
+ * 
+ * Typically sent by a ground control station / joystick or by
+ * some off-board controller via C or SIMULINK.
+ */
+enum OFFBOARD_CONTROL_MODE
+{
+	OFFBOARD_CONTROL_MODE_DIRECT = 0,
+	OFFBOARD_CONTROL_MODE_DIRECT_RATES = 1,
+	OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE = 2,
+	OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY = 3,
+	OFFBOARD_CONTROL_MODE_DIRECT_POSITION = 4,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_RATE = 5,
+	OFFBOARD_CONTROL_MODE_ATT_YAW_POS = 6,
+	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
+};
+
+struct offboard_control_setpoint_s {
+	uint64_t timestamp;
+
+	enum OFFBOARD_CONTROL_MODE mode;		 /**< The current control inputs mode */
+	bool armed;	/**< Armed flag set, yes / no */
+	float p1;	/**< ailerons roll / roll rate input */
+	float p2;	/**< elevator / pitch / pitch rate */
+	float p3;	/**< rudder / yaw rate / yaw */
+	float p4;	/**< throttle / collective thrust / altitude */
+
+	float override_mode_switch;
+
+	float aux1_cam_pan_flaps;
+	float aux2_cam_tilt;
+	float aux3_cam_zoom;
+	float aux4_cam_roll;
+
+}; /**< offboard control inputs */
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(offboard_control_setpoint);
+
+#endif
diff --git a/apps/uORB/topics/vehicle_rates_setpoint.h b/apps/uORB/topics/vehicle_rates_setpoint.h
index 3400e5399..46e62c4b7 100644
--- a/apps/uORB/topics/vehicle_rates_setpoint.h
+++ b/apps/uORB/topics/vehicle_rates_setpoint.h
@@ -47,7 +47,6 @@
  * @addtogroup topics
  * @{
  */
-
 struct vehicle_rates_setpoint_s
 {
 	uint64_t timestamp; /**< in microseconds since system start */
diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h
index 850029f01..c727527b1 100644
--- a/apps/uORB/topics/vehicle_status.h
+++ b/apps/uORB/topics/vehicle_status.h
@@ -110,6 +110,8 @@ struct vehicle_status_s
 
 	uint16_t counter;   /**< incremented by the writing thread everytime new data is stored */
 	uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */
+	uint64_t failsave_lowlevel_start_time;		/**< time when the lowlevel failsafe flag was set */
+	//uint64_t failsave_highlevel_begin; TO BE COMPLETED
 
 	commander_state_machine_t state_machine;	/**< current flight state, main state machine */
 	enum VEHICLE_FLIGHT_MODE flight_mode;		/**< current flight mode, as defined by mode switch */
@@ -134,10 +136,18 @@ struct vehicle_status_s
 	bool flag_preflight_mag_calibration;			/**< true if mag calibration is requested */
 	bool flag_preflight_accel_calibration;
 
+	bool rc_signal_found_once;
 	bool rc_signal_lost;				/**< true if RC reception is terminally lost */
 	bool rc_signal_cutting_off;			/**< true if RC reception is weak / cutting off */
 	uint64_t rc_signal_lost_interval;		/**< interval in microseconds since when no RC signal is available */
 
+	bool offboard_control_signal_found_once;
+	bool offboard_control_signal_lost;
+	uint64_t offboard_control_signal_lost_interval;	/**< interval in microseconds without an offboard control message */
+
+	bool failsave_lowlevel;				/**< Set to true if low-level failsafe mode is enabled */
+	//bool failsave_highlevel;
+
 	/* see SYS_STATUS mavlink message for the following */
 	uint32_t onboard_control_sensors_present;
 	uint32_t onboard_control_sensors_enabled;