From 93f45f232f1a1a195b8566818869bf93bd3a4476 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Mon, 9 Jul 2012 11:49:26 +1000 Subject: [PATCH] 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 | 14 ++++ ArduPlane/Makefile | 5 ++ ArduPlane/Parameters.h | 3 +- ArduPlane/Parameters.pde | 4 + ArduPlane/config.h | 5 ++ libraries/APM_OBC/APM_OBC.cpp | 152 ++++++++++++++++++++++++++++++++++ libraries/APM_OBC/APM_OBC.h | 83 +++++++++++++++++++ 7 files changed, 265 insertions(+), 1 deletion(-) create mode 100644 libraries/APM_OBC/APM_OBC.cpp create mode 100644 libraries/APM_OBC/APM_OBC.h diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 3bb5b540a0..1dac6ac636 100644 --- a/ArduPlane/ArduPlane.pde +++ b/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 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; } } diff --git a/ArduPlane/Makefile b/ArduPlane/Makefile index 1f690ce374..06cacb7628 100644 --- a/ArduPlane/Makefile +++ b/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" diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h index a8d013d35e..c86bed0704 100644 --- a/ArduPlane/Parameters.h +++ b/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 diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde index 7b3518120d..0c92a1310b 100644 --- a/ArduPlane/Parameters.pde +++ b/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 }; diff --git a/ArduPlane/config.h b/ArduPlane/config.h index 65d3286a8a..9563d5fa5a 100644 --- a/ArduPlane/config.h +++ b/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 diff --git a/libraries/APM_OBC/APM_OBC.cpp b/libraries/APM_OBC/APM_OBC.cpp new file mode 100644 index 0000000000..529527c6e0 --- /dev/null +++ b/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 +#include + +// 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); + } +} diff --git a/libraries/APM_OBC/APM_OBC.h b/libraries/APM_OBC/APM_OBC.h new file mode 100644 index 0000000000..df9df1d8f5 --- /dev/null +++ b/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 +#include + + +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