Failsafe: prototype of APM2 based failsafe system

Tools/Failsafe/Failsafe.pde

@@ -0,0 +1,98 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/*
+This firmware 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.
+*/
+
+// Libraries
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <APM_RC.h>         // ArduPilot Mega RC Library
+
+////////////////////////////////////////////////////////////////////////////////
+// Serial ports
+FastSerialPort0(Serial);        // FTDI/console
+
+APM_RC_APM2 APM_RC;
+Arduino_Mega_ISR_Registry isr_registry;
+
+#define HEARTBEAT_PIN A0
+#define FAILSAFE_PIN  A1
+
+void setup() {
+	Serial.begin(115200, 256, 256);
+	isr_registry.init();
+	APM_RC.Init(&isr_registry);		// APM Radio initialization
+	pinMode(HEARTBEAT_PIN, INPUT);
+	pinMode(FAILSAFE_PIN, INPUT);
+
+	// enable all output channels
+	for (uint8_t ch=0; ch<8; ch++) {
+		APM_RC.enable_out(ch);
+	}
+	Serial.printf("Failsafe board started\n");
+}    
+
+
+static uint8_t last_heartbeat;
+static uint32_t last_heartbeat_change_t;
+static bool heartbeat_failure;
+static uint32_t last_print;
+
+void loop()
+{
+	uint16_t pwm[8];
+	bool failsafe_enabled;
+	uint32_t tnow = millis();
+	uint8_t heartbeat_pin = digitalRead(HEARTBEAT_PIN);
+
+	// to minimise latency run the main loop at 200Hz
+	delay(5);
+
+	if (heartbeat_pin != last_heartbeat) {
+		last_heartbeat = heartbeat_pin;
+		last_heartbeat_change_t = tnow;
+	}
+	bool hb_failed = (tnow - last_heartbeat_change_t > 500);
+	if (hb_failed != heartbeat_failure) {
+		heartbeat_failure = hb_failed;
+		Serial.printf("HBFAIL=%u\n", (unsigned)heartbeat_failure);
+	}
+
+	// ch0 to ch3 come from the autopilot
+	// ch4 to ch7 come from the receiver
+
+	for (uint8_t ch=0; ch<8; ch++) {
+		pwm[ch] = APM_RC.InputCh(ch);
+	}
+	failsafe_enabled = digitalRead(FAILSAFE_PIN);
+	if (tnow - last_print > 200) {
+		Serial.printf("1:%4u 2:%4u 3:%4u 4:%4u 5:%4u 6:%4u 7:%4u 8:%4u HB=%u FS=%u HBFAIL=%u\n",
+			      pwm[0], pwm[1], pwm[2], pwm[3], 
+			      pwm[4], pwm[5], pwm[6], pwm[7],
+			      (unsigned)digitalRead(HEARTBEAT_PIN),
+			      (unsigned)failsafe_enabled,
+			      (unsigned)heartbeat_failure);
+		last_print = tnow;
+	}
+	if (failsafe_enabled || heartbeat_failure) {
+		// if we are in failover, send receiver input straight
+		// to the servos
+		for (uint8_t ch=0; ch<4; ch++) {
+			APM_RC.OutputCh(ch, pwm[ch+4]);
+		}
+	} else {
+		// otherwise send autopilot output to the servos
+		for (uint8_t ch=0; ch<4; ch++) {
+			APM_RC.OutputCh(ch, pwm[ch]);
+		}
+	}
+	// always pass the receiver input to the autopilot
+	for (uint8_t ch=4; ch<8; ch++) {
+		APM_RC.OutputCh(ch, pwm[ch]);
+	}
+}

Tools/Failsafe/Makefile

@@ -0,0 +1,6 @@
+#
+# Trivial makefile for building APM
+#
+BOARD=mega
+include ../../libraries/AP_Common/Arduino.mk
+