APM: added 'OBC' failsafe module for ArduPlane

this adds FS_* parameters for setting up APM to follow the outback
challenge failsafe rules.

This includes:

 - manual pin
 - heartbeat pin
 - waypoint for heartbeat failure
 - waypoint for GPS failure

ArduPlane/ArduPlane.pde

@@ -89,6 +89,12 @@ FastSerialPort1(Serial1);       // GPS port
 // variables
 AP_Param param_loader(var_info, WP_START_BYTE);
 
+// Outback Challenge failsafe support
+#if OBC_FAILSAFE == ENABLED
+#include <APM_OBC.h>
+APM_OBC obc;
+#endif
+
 
 ////////////////////////////////////////////////////////////////////////////////
 // ISR Registry
@@ -915,6 +921,14 @@ Serial.println(tempaccel.z, DEC);
 			}
 
 			slow_loop();
+
+#if OBC_FAILSAFE == ENABLED
+            // perform OBC failsafe checks
+            obc.check(OBC_MODE(control_mode),
+                      last_heartbeat_ms,
+                      g_gps?g_gps->last_fix_time:0);
+#endif
+
 			break;
 	}
 }

ArduPlane/Makefile

@@ -46,3 +46,8 @@ sitl-quaternion:
 etags:
 	cd .. && etags -f ArduPlane/TAGS --langmap=C++:.pde.cpp.h $$(git ls-files ArduPlane libraries)
 
+obc:
+	make -f Makefile EXTRAFLAGS="-DCONFIG_APM_HARDWARE=APM_HARDWARE_APM2 -DOBC_FAILSAFE=ENABLED -DTELEMETRY_UART2=ENABLED"
+
+obc-sitl:
+	make -f ../libraries/Desktop/Makefile.desktop EXTRAFLAGS="-DOBC_FAILSAFE=ENABLED"

ArduPlane/Parameters.h

@@ -185,8 +185,9 @@ public:
         k_param_fence_minalt,
         k_param_fence_maxalt,
 
-        // simulator control
+        // other objects
         k_param_sitl = 230,
+        k_param_obc,
 
         //
         // 240: PID Controllers

ArduPlane/Parameters.pde

@@ -564,6 +564,10 @@ const AP_Param::Info var_info[] PROGMEM = {
 	GOBJECT(sitl, "SIM_", SITL),
 #endif
 
+#if OBC_FAILSAFE == ENABLED
+	GOBJECT(obc,  "FS_", APM_OBC),
+#endif
+
 	AP_VAREND
 };
 

ArduPlane/config.h

@@ -843,3 +843,8 @@
 #ifndef DMP_ENABLED
 # define DMP_ENABLED DISABLED
 #endif
+
+// OBC Failsafe enable
+#ifndef OBC_FAILSAFE
+# define OBC_FAILSAFE DISABLED
+#endif

libraries/APM_OBC/APM_OBC.cpp

@@ -0,0 +1,152 @@
+/*
+	APM_OBC.cpp
+
+	Outback Challenge Failsafe module
+
+	This library is free software; you can redistribute it and/or
+	modify it under the terms of the GNU Lesser General Public License
+	as published by the Free Software Foundation; either version 2.1
+	of the License, or (at your option) any later version.
+*/
+#include <FastSerial.h>
+#include <APM_OBC.h>
+
+// table of user settable parameters
+const AP_Param::GroupInfo APM_OBC::var_info[] PROGMEM = {
+    // @Param: MAN_PIN
+    // @DisplayName: Manual Pin
+    // @Description: This sets a digital output pin to set high when in manual mode
+    // @User: Advanced
+    AP_GROUPINFO("MAN_PIN",     0, APM_OBC, _manual_pin,    -1),
+
+    // @Param: HB_PIN
+    // @DisplayName: Heartbeat Pin
+    // @Description: This sets a digital output pin which is cycled at 10Hz when termination is not activated
+    // @User: Advanced
+    AP_GROUPINFO("HB_PIN",      1, APM_OBC, _heartbeat_pin, -1),
+
+    // @Param: WP_COMMS
+    // @DisplayName: Comms Waypoint
+    // @Description: Waypoint number to navigate to on comms loss
+    // @User: Advanced
+    AP_GROUPINFO("WP_COMMS",    2, APM_OBC, _wp_comms_hold, 0),
+
+    // @Param: GPS_LOSS
+    // @DisplayName: GPS Loss Waypoint
+    // @Description: Waypoint number to navigate to on GPS lock loss
+    // @User: Advanced
+    AP_GROUPINFO("WP_GPS_LOSS", 4, APM_OBC, _wp_gps_loss, 0),
+
+    // @Param: TERMINATE
+    // @DisplayName: Force Terminate
+    // @Description: Can be set in flight to force termination of the heartbeat signal
+    // @User: Advanced
+    AP_GROUPINFO("TERMINATE",   5, APM_OBC, _terminate, 0),
+
+    AP_GROUPEND
+};
+
+// access to geofence state
+extern bool geofence_breached(void);
+
+// function to change waypoint
+extern void change_command(uint8_t cmd_index);
+
+// for sending messages
+extern void gcs_send_text_fmt(const prog_char_t *fmt, ...);
+
+// check for Failsafe conditions. This is called at 10Hz by the main
+// ArduPlane code
+void
+APM_OBC::check(APM_OBC::control_mode mode,
+	       uint32_t last_heartbeat_ms,
+	       uint32_t last_gps_fix_ms)
+{	
+	// we always check for fence breach
+	if (geofence_breached()) {
+		if (!_terminate) {
+			gcs_send_text_fmt(PSTR("Fence TERMINATE"));
+			_terminate.set(1);
+		}
+	}
+	
+	// tell the failsafe board if we are in manual control
+	// mode. This tells it to pass through controls from the
+	// receiver
+	if (_manual_pin != -1) {
+		digitalWrite(_manual_pin, mode==OBC_MANUAL);
+	}
+
+	uint32_t now = millis();
+	bool gcs_link_ok = ((now - last_heartbeat_ms) < 10000);
+	bool gps_lock_ok = ((now - last_gps_fix_ms) < 3000);
+
+	switch (_state) {
+	case STATE_PREFLIGHT:
+		// we startup in preflight mode. This mode ends when
+		// we first enter auto.
+		if (mode == OBC_AUTO) {
+			gcs_send_text_fmt(PSTR("Starting OBC_AUTO"));
+			_state = STATE_AUTO;
+		}
+		break;
+
+	case STATE_AUTO:
+		// this is the normal mode. 
+		if (!gcs_link_ok) {
+			gcs_send_text_fmt(PSTR("State DATA_LINK_LOSS"));
+			_state = STATE_DATA_LINK_LOSS;
+			if (_wp_comms_hold) {
+				if (_command_index != NULL) {
+					_saved_wp = _command_index->get();
+				}
+				change_command(_wp_comms_hold);
+			}
+			break;
+		}
+		if (!gps_lock_ok) {
+			gcs_send_text_fmt(PSTR("State GPS_LOSS"));
+			_state = STATE_GPS_LOSS;
+			if (_wp_gps_loss) {
+				if (_command_index != NULL) {
+					_saved_wp = _command_index->get() + 1;
+				}
+				change_command(_wp_gps_loss);
+			}
+			break;
+		}
+		break;
+
+	case STATE_DATA_LINK_LOSS:
+		if (!gps_lock_ok) {
+			// losing GPS lock when data link is lost
+			// leads to termination
+			gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
+			_terminate.set(1);
+		} else if (gcs_link_ok) {
+			_state = STATE_AUTO;
+			gcs_send_text_fmt(PSTR("GCS OK"));
+			change_command(_saved_wp);			
+		}
+		break;
+
+	case STATE_GPS_LOSS:
+		if (!gcs_link_ok) {
+			// losing GCS link when GPS lock lost
+			// leads to termination
+			gcs_send_text_fmt(PSTR("Dual loss TERMINATE"));
+			_terminate.set(1);
+		} else if (gps_lock_ok) {
+			gcs_send_text_fmt(PSTR("GPS OK"));
+			_state = STATE_AUTO;
+			change_command(_saved_wp);			
+		}
+		break;
+	}
+
+	// if we are not terminating then toggle the heartbeat pin at 10Hz
+	if (!_terminate && _heartbeat_pin != -1) {
+		_heartbeat_pin_value = !_heartbeat_pin_value;
+		digitalWrite(_heartbeat_pin, _heartbeat_pin_value);
+	}	
+}

libraries/APM_OBC/APM_OBC.h

@@ -0,0 +1,83 @@
+#ifndef APM_OBC_H
+#define APM_OBC_H
+/*
+  Outback Challenge Failsafe module
+
+  Andrew Tridgell and CanberraUAV, August 2012
+
+  This library is free software; you can redistribute it and/or
+  modify it under the terms of the GNU Lesser General Public
+  License as published by the Free Software Foundation; either
+  version 2.1 of the License, or (at your option) any later version.
+*/
+
+#include <AP_Common.h>
+#include <inttypes.h>
+
+
+class APM_OBC
+{
+public:
+	enum control_mode {
+		OBC_MANUAL = 0,
+		OBC_FBW    = 1,
+		OBC_AUTO   = 2
+	};
+
+	enum state {
+		STATE_PREFLIGHT       = 0,
+		STATE_AUTO            = 1,
+		STATE_DATA_LINK_LOSS  = 2,
+		STATE_GPS_LOSS        = 3
+	};
+
+	// Constructor
+	APM_OBC(void)
+	{
+		if (_heartbeat_pin != -1) {
+			pinMode(_heartbeat_pin, OUTPUT);
+		}
+		if (_manual_pin != -1) {
+			pinMode(_manual_pin,    OUTPUT);
+		}
+		_state = STATE_PREFLIGHT;
+		_terminate.set(0);
+
+		// get a pointer to COMMAND_INDEX so we can resume a
+		// auto mission when a failsafe condition is resolved
+		enum ap_var_type var_type;
+		_command_index = (AP_Int8 *)AP_Param::find("CMD_INDEX", &var_type);
+	}
+
+	void check(enum control_mode control_mode,
+		   uint32_t last_heartbeat_ms,
+		   uint32_t last_gps_fix_ms);
+
+	// for holding parameters
+	static const struct AP_Param::GroupInfo var_info[];
+
+private:
+	enum state _state;
+
+	// digital output pins for communicating with the failsafe board
+	AP_Int8 _heartbeat_pin;
+	AP_Int8 _manual_pin;
+	AP_Int8 _terminate;
+
+	// waypoint numbers to jump to on failsafe conditions
+	AP_Int8 _wp_comms_hold;
+	AP_Int8 _wp_gps_loss;
+
+	bool _heartbeat_pin_value;
+
+	// pointer to command index parameter in g
+	AP_Int8 *_command_index;
+
+	// saved waypoint for resuming mission
+	uint8_t _saved_wp;
+};
+
+// map from ArduPlane control_mode to APM_OBC::control_mode
+#define OBC_MODE(control_mode) ((control_mode==AUTO?APM_OBC::OBC_AUTO:control_mode==MANUAL?APM_OBC::OBC_MANUAL:APM_OBC::OBC_FBW))
+
+#endif // APM_OBC_H