APM_OBM: Failsafe board sketch

this combines with the OBC failsafe module

libraries/APM_OBC/Failsafe_Board/Failsafe_Board.pde

+/*
+  OBC failsafe board
+
+  Jack Pittar and Andrew Tridgell
+
+  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.
+*/
+
+#define MODE_PWM_PIN 9 //Command pulse read on input 11
+#define MUXA_PIN     3  // H H rc    , L H micro
+#define MUXB_PIN     4  // H L backup, L L prime
+#define LED_PIN      13
+#define OBC_MODE_PIN 12 //high for OBC termination settings
+#define AILERON_PIN  5
+#define ELEVATOR_PIN 6
+#define THROTTLE_PIN 7
+#define RUDDER_PIN   8
+
+#define HEARTBEAT_PRIMARY_PIN 11
+#define TERMINATE_PRIMARY_PIN 10
+
+#define HEARTBEAT_BACKUP_PIN A1
+#define TERMINATE_BACKUP_PIN A0
+
+// set to 1 if backup autopilot is installed
+#define BACKUP_AUTOPILOT_INSTALLED 0
+
+enum mux_mode {
+	MUX_MODE_RC      = 1,
+	MUX_MODE_MICRO   = 2,
+	MUX_MODE_BACKUP  = 3,
+	MUX_MODE_PRIMARY = 4
+};
+
+// heartbeat status
+static bool heartbeat_primary_ok   = true;
+static bool heartbeat_backup_ok    = true;
+
+
+
+// check heartbeat status
+static void update_heartbeat(void)
+{
+	static uint8_t last_hb_primary;
+	static uint8_t last_hb_backup;
+	static uint32_t last_primary_t;
+	static uint32_t last_backup_t;
+	uint8_t hb_primary   = digitalRead(HEARTBEAT_PRIMARY_PIN);
+	uint8_t hb_backup    = digitalRead(HEARTBEAT_BACKUP_PIN);
+	uint32_t tnow = millis();
+//	Serial.println(hb_primary);
+	// we consider the heartbeat to be OK if it changed in the
+	// last 0.4 seconds
+	const uint32_t hb_check_time = 400;
+	if (hb_primary != last_hb_primary) {
+		last_hb_primary = hb_primary;
+		last_primary_t = tnow;
+		//Serial.println("hb1 change");
+	}
+	if (hb_backup != last_hb_backup) {
+		last_hb_backup = hb_backup;
+		last_backup_t = tnow;
+		//Serial.println("hb2 change");
+	}
+	if (tnow < hb_check_time) {
+		// not enough time has passed for a measurement
+		return;
+	}
+	heartbeat_primary_ok   = ((tnow - last_primary_t)   < hb_check_time);
+	heartbeat_backup_ok = ((tnow - last_backup_t) < hb_check_time);
+}
+
+
+static void set_mux_mode(uint8_t mode)
+{
+	switch ((enum mux_mode)mode) {
+	case MUX_MODE_RC:
+		digitalWrite(MUXA_PIN, HIGH);
+		digitalWrite(MUXB_PIN, HIGH);
+		break;
+	case MUX_MODE_MICRO:
+		digitalWrite(MUXA_PIN, LOW);
+		digitalWrite(MUXB_PIN, HIGH);
+		break;
+	case MUX_MODE_BACKUP:
+		digitalWrite(MUXA_PIN, HIGH);
+		digitalWrite(MUXB_PIN, LOW);
+		break;
+	case MUX_MODE_PRIMARY:
+		digitalWrite(MUXA_PIN, LOW);
+		digitalWrite(MUXB_PIN, LOW);
+		break;		
+	}
+}
+
+// send pulses to servos
+static void set_servos(uint16_t aileron, uint16_t elevator,
+		       uint16_t throttle, uint16_t rudder)
+{
+	digitalWrite(AILERON_PIN, HIGH);          
+	delayMicroseconds(aileron);
+	digitalWrite(AILERON_PIN, LOW);  
+
+	digitalWrite(ELEVATOR_PIN, HIGH);
+	delayMicroseconds(elevator);
+	digitalWrite(ELEVATOR_PIN, LOW);
+
+	digitalWrite(THROTTLE_PIN, HIGH);
+	delayMicroseconds(throttle);
+	digitalWrite(THROTTLE_PIN, LOW); 
+
+	digitalWrite(RUDDER_PIN, HIGH);
+	delayMicroseconds(rudder);
+	digitalWrite(RUDDER_PIN, LOW);
+}
+
+static void set_servos_terminate(uint8_t obc_mode)
+{
+	set_mux_mode(MUX_MODE_MICRO);
+	if (obc_mode) {
+		set_servos(2000, 2000, 1000, 2000);
+	} else {
+		set_servos(1500, 1500, 1200, 1500);
+	}
+}
+
+
+void setup() 
+{
+	// input pins
+	pinMode(HEARTBEAT_PRIMARY_PIN, INPUT);
+	pinMode(TERMINATE_PRIMARY_PIN, INPUT);
+	pinMode(HEARTBEAT_BACKUP_PIN, INPUT);
+	pinMode(TERMINATE_BACKUP_PIN, INPUT);
+	pinMode(MODE_PWM_PIN, INPUT);
+	pinMode(OBC_MODE_PIN, INPUT);
+
+	// output pins
+	pinMode(LED_PIN, OUTPUT);
+	pinMode(MUXA_PIN, OUTPUT);
+	pinMode(MUXB_PIN, OUTPUT);
+	pinMode(AILERON_PIN, OUTPUT); 
+	pinMode(ELEVATOR_PIN, OUTPUT);
+	pinMode(THROTTLE_PIN, OUTPUT);
+	pinMode(RUDDER_PIN, OUTPUT);   
+
+	digitalWrite(LED_PIN, LOW);
+	set_mux_mode(MUX_MODE_MICRO);
+
+	Serial.begin(115200);
+	Serial.println("OBC Failsafe Starting\n");
+}   
+
+void loop() 
+{
+	static uint32_t last_status_t;
+	uint32_t tnow = millis();
+	static uint8_t led_state;
+	static bool has_terminated = false;
+	static uint8_t termination_counter;
+
+	// check for heartbeat
+	update_heartbeat();
+
+	// see if we are in manual mode. Note that on the Uno,
+	// pulseIn is not very accurate. The +90 brings us much closer
+	// to the real pulse width
+	uint16_t mode_pwm = pulseIn(MODE_PWM_PIN, HIGH, 25000) + 90;
+	uint16_t manual_mode = (mode_pwm > 1750 && mode_pwm < 2100);
+
+	// check the state of the terminate pins
+	uint8_t terminate_primary = digitalRead(TERMINATE_PRIMARY_PIN);
+	uint8_t terminate_backup  = digitalRead(TERMINATE_BACKUP_PIN);
+
+	// see if the OBC mode jumper is set
+	uint8_t obc_mode = digitalRead(OBC_MODE_PIN);
+
+	if (tnow - last_status_t > 1000) {
+		last_status_t = tnow;
+		// show status once a second
+		Serial.print("OBC:"); Serial.print(obc_mode);
+		Serial.print(" Mode:"); Serial.print(mode_pwm);
+		Serial.print(" HB1:"); Serial.print(heartbeat_primary_ok);
+		Serial.print(" HB2:"); Serial.print(heartbeat_backup_ok);
+		Serial.print(" TERM1:"); Serial.print(terminate_primary);
+		Serial.print(" TERM2:"); Serial.print(terminate_backup);
+		Serial.print(" TERMINATED:"); Serial.print(has_terminated);
+		Serial.println();
+		delayMicroseconds(5000);
+		// flash LED once a second so we know failsafe board
+		// is working
+		led_state = !led_state;
+		digitalWrite(LED_PIN, led_state);
+	}
+
+	// allow reset via serial connection
+	if (Serial.available() > 0) {
+		char c = (char)Serial.read();
+		if (c == 'r') {
+			Serial.println("RESET BOARD");
+			has_terminated = false;
+			termination_counter = 0;
+		}
+	}
+
+	// if we are not in OBC mode, and the mode control 
+	// channel is high, then give RC control
+	if (!obc_mode && manual_mode) {
+		// give manual control via RC
+		set_mux_mode(MUX_MODE_RC);		
+		return;
+	}
+
+#if BACKUP_AUTOPILOT_INSTALLED == 0
+	terminate_backup = false;
+#endif
+
+	// see if termination is set by a functioning autopilot
+	if (heartbeat_primary_ok && terminate_primary) {
+		termination_counter++;
+	} else if (heartbeat_backup_ok && terminate_backup) {
+		termination_counter++;
+	} else if (obc_mode && !heartbeat_primary_ok && !heartbeat_backup_ok) {
+		// if in OBC mode and neither autopilot is OK, then
+		// terminate
+		termination_counter++;
+	} else {
+		termination_counter = 0;
+	}
+
+	// use the termination counter to debounce the termination
+	// pins
+	if (termination_counter > 10) {
+		termination_counter = 0;
+		has_terminated = true;
+	}
+
+	// if we have terminated then setup the servos
+	if (has_terminated) {
+		set_servos_terminate(obc_mode);
+		return;		
+	}
+
+	bool heartbeat_ok = heartbeat_primary_ok;
+#if BACKUP_AUTOPILOT_INSTALLED
+	if (heartbeat_backup_ok) {
+		heartbeat_ok = true;
+	}
+#endif
+
+	if (heartbeat_ok) {
+		// at least one autopilot is healthy
+		if (manual_mode) {
+			// we want manual/RC control
+			set_mux_mode(MUX_MODE_RC);
+		} else if (heartbeat_primary_ok) {
+			set_mux_mode(MUX_MODE_PRIMARY);
+		} else {
+			set_mux_mode(MUX_MODE_BACKUP);
+		}
+	} else {
+		// neither autopilot is OK
+		if (obc_mode) {
+			set_servos_terminate(obc_mode);
+		} else {
+			// give RC control 
+			set_mux_mode(MUX_MODE_RC);
+		}
+	}
+}
+

libraries/APM_OBC/Failsafe_Board/Makefile

+#
+# Trivial makefile for building APM
+#
+BOARD=uno
+include ../../../libraries/AP_Common/Arduino.mk
+