From 570470ac594c5958efd1695c23b9f827779f8c4d Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 5 Jul 2018 13:59:05 +1000 Subject: [PATCH] AP_AdvancedFailsafe: removed some old AVR code --- .../Failsafe_Board/Failsafe_Board.pde | 290 ------------------ .../Failsafe_Board/Makefile | 6 - .../Failsafe_Board/nobuild.txt | 0 3 files changed, 296 deletions(-) delete mode 100644 libraries/AP_AdvancedFailsafe/Failsafe_Board/Failsafe_Board.pde delete mode 100644 libraries/AP_AdvancedFailsafe/Failsafe_Board/Makefile delete mode 100644 libraries/AP_AdvancedFailsafe/Failsafe_Board/nobuild.txt diff --git a/libraries/AP_AdvancedFailsafe/Failsafe_Board/Failsafe_Board.pde b/libraries/AP_AdvancedFailsafe/Failsafe_Board/Failsafe_Board.pde deleted file mode 100644 index ec91defe69..0000000000 --- a/libraries/AP_AdvancedFailsafe/Failsafe_Board/Failsafe_Board.pde +++ /dev/null @@ -1,290 +0,0 @@ -/* - This program is free software: you can redistribute it and/or modify - it under the terms of the GNU General Public License as published by - the Free Software Foundation, either version 3 of the License, or - (at your option) any later version. - - This program is distributed in the hope that it will be useful, - but WITHOUT ANY WARRANTY; without even the implied warranty of - MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - GNU General Public License for more details. - - You should have received a copy of the GNU General Public License - along with this program. If not, see . - */ - -/* - OBC failsafe board - - Jack Pittar and Andrew Tridgell - -*/ - -#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(1000, 2000, 1000, 1000); - } 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; - static uint16_t loop_counter; - - loop_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.print(" LOOP:"); Serial.print(loop_counter); - Serial.println(); - delayMicroseconds(5000); - loop_counter = 0; - // 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); - } - } -} - diff --git a/libraries/AP_AdvancedFailsafe/Failsafe_Board/Makefile b/libraries/AP_AdvancedFailsafe/Failsafe_Board/Makefile deleted file mode 100644 index 8bf31e7de3..0000000000 --- a/libraries/AP_AdvancedFailsafe/Failsafe_Board/Makefile +++ /dev/null @@ -1,6 +0,0 @@ -# -# Trivial makefile for building APM -# -BOARD=uno -include ../../../libraries/../mk/apm.mk - diff --git a/libraries/AP_AdvancedFailsafe/Failsafe_Board/nobuild.txt b/libraries/AP_AdvancedFailsafe/Failsafe_Board/nobuild.txt deleted file mode 100644 index e69de29bb2..0000000000