From 99ba8bdc08a9357f7afb154c0f5f6c0e7a29fd30 Mon Sep 17 00:00:00 2001 From: Pierre Kancir Date: Mon, 30 Jan 2017 15:21:55 +0100 Subject: [PATCH] APMRover2: add advance failsafe --- APMrover2/APMrover2.cpp | 3 ++ APMrover2/Parameters.cpp | 11 +++++++- APMrover2/Parameters.h | 6 ++++ APMrover2/Rover.h | 9 ++++++ APMrover2/afs_rover.cpp | 60 ++++++++++++++++++++++++++++++++++++++++ APMrover2/afs_rover.h | 43 ++++++++++++++++++++++++++++ APMrover2/config.h | 4 +++ APMrover2/failsafe.cpp | 10 +++++++ APMrover2/wscript | 1 + 9 files changed, 146 insertions(+), 1 deletion(-) create mode 100644 APMrover2/afs_rover.cpp create mode 100644 APMrover2/afs_rover.h diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 6265e07289..d97bab6bcc 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -77,6 +77,9 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = { SCHED_TASK(button_update, 5, 100), SCHED_TASK(stats_update, 1, 100), SCHED_TASK(crash_check, 10, 1000), +#if ADVANCED_FAILSAFE == ENABLED + SCHED_TASK(afs_fs_check, 10, 100), +#endif }; /* diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp index 2f3f0d3982..fe62279318 100644 --- a/APMrover2/Parameters.cpp +++ b/APMrover2/Parameters.cpp @@ -536,12 +536,21 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { // @Group: RC // @Path: ../libraries/RC_Channel/RC_Channel.cpp AP_SUBGROUPINFO(rc_channels, "RC", 4, ParametersG2, RC_Channels), - + +#if ADVANCED_FAILSAFE == ENABLED + // @Group: AFS_ + // @Path: ../libraries/AP_AdvancedFailsafe/AP_AdvancedFailsafe.cpp + AP_SUBGROUPINFO(afs, "AFS_", 5, ParametersG2, AP_AdvancedFailsafe), +#endif + AP_GROUPEND }; ParametersG2::ParametersG2(void) +#if ADVANCED_FAILSAFE == ENABLED + : afs(rover.mission, rover.barometer, rover.gps, rover.rcmap) +#endif { AP_Param::setup_object_defaults(this, var_info); } diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h index 42742db513..3e2964e5f1 100644 --- a/APMrover2/Parameters.h +++ b/APMrover2/Parameters.h @@ -313,6 +313,12 @@ public: // control over servo output ranges SRV_Channels servo_channels; + +#if ADVANCED_FAILSAFE == ENABLED + // advanced failsafe library + AP_AdvancedFailsafe_Rover afs; +#endif + }; extern const AP_Param::Info var_info[]; diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 386ac83281..62c1ff9063 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -79,6 +79,9 @@ // Local modules #include "defines.h" +#if ADVANCED_FAILSAFE == ENABLED +#include "afs_rover.h" +#endif #include "Parameters.h" #include "GCS_Mavlink.h" @@ -94,6 +97,9 @@ public: friend class Parameters; friend class ParametersG2; friend class AP_Arming_Rover; +#if ADVANCED_FAILSAFE == ENABLED + friend class AP_AdvancedFailsafe_Rover; +#endif Rover(void); @@ -554,6 +560,9 @@ private: void set_loiter_active(const AP_Mission::Mission_Command& cmd); void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target); void crash_check(); +#if ADVANCED_FAILSAFE == ENABLED + void afs_fs_check(void); +#endif public: bool print_log_menu(void); diff --git a/APMrover2/afs_rover.cpp b/APMrover2/afs_rover.cpp new file mode 100644 index 0000000000..c08c9ff6dc --- /dev/null +++ b/APMrover2/afs_rover.cpp @@ -0,0 +1,60 @@ +/* + Rover specific AP_AdvancedFailsafe class + */ + +#include "Rover.h" + +#if ADVANCED_FAILSAFE == ENABLED + +// Constructor +AP_AdvancedFailsafe_Rover::AP_AdvancedFailsafe_Rover(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap) : + AP_AdvancedFailsafe(_mission, _baro, _gps, _rcmap) +{} + + +/* + Setup radio_out values for all channels to termination values + */ +void AP_AdvancedFailsafe_Rover::terminate_vehicle(void) +{ + // stop motors + SRV_Channels::set_output_pwm(SRV_Channel::k_throttle, 0); + SRV_Channels::set_output_pwm(SRV_Channel::k_steering, 0); + rover.lateral_acceleration = 0; + + // disarm as well + rover.disarm_motors(); + + // Set to HOLD mode + rover.set_mode(HOLD); + // and set all aux channels + SRV_Channels::set_output_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_output_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + + SRV_Channels::output_ch_all(); +} + +void AP_AdvancedFailsafe_Rover::setup_IO_failsafe(void) +{ + // setup failsafe for all aux channels + SRV_Channels::set_failsafe_limit(SRV_Channel::k_manual, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); + SRV_Channels::set_failsafe_limit(SRV_Channel::k_none, SRV_Channel::SRV_CHANNEL_LIMIT_TRIM); +} + +/* + Return an AFS_MODE for current control mode + */ +AP_AdvancedFailsafe::control_mode AP_AdvancedFailsafe_Rover::afs_mode(void) +{ + switch (rover.control_mode) { + case AUTO: + case GUIDED: + case RTL: + return AP_AdvancedFailsafe::AFS_AUTO; + default: + break; + } + return AP_AdvancedFailsafe::AFS_STABILIZED; +} + +#endif // ADVANCED_FAILSAFE diff --git a/APMrover2/afs_rover.h b/APMrover2/afs_rover.h new file mode 100644 index 0000000000..60bc96ab63 --- /dev/null +++ b/APMrover2/afs_rover.h @@ -0,0 +1,43 @@ +/* + 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 . + */ +#pragma once + +/* + advanced failsafe support for rover + */ + +#if ADVANCED_FAILSAFE == ENABLED +#include + +/* + a rover specific AP_AdvancedFailsafe class + */ +class AP_AdvancedFailsafe_Rover : public AP_AdvancedFailsafe +{ +public: + AP_AdvancedFailsafe_Rover(AP_Mission &_mission, AP_Baro &_baro, const AP_GPS &_gps, const RCMapper &_rcmap); + + // called to set all outputs to termination state + void terminate_vehicle(void); + +protected: + // setup failsafe values for if FMU firmware stops running + void setup_IO_failsafe(void); + + // return the AFS mapped control mode + enum control_mode afs_mode(void); +}; + +#endif // ADVANCED_FAILSAFE diff --git a/APMrover2/config.h b/APMrover2/config.h index 7382298688..3b5131cc3c 100644 --- a/APMrover2/config.h +++ b/APMrover2/config.h @@ -255,3 +255,7 @@ #ifndef RESET_SWITCH_CHAN_PWM #define RESET_SWITCH_CHAN_PWM 1750 #endif + +#ifndef ADVANCED_FAILSAFE + #define ADVANCED_FAILSAFE DISABLED +#endif diff --git a/APMrover2/failsafe.cpp b/APMrover2/failsafe.cpp index b749bc984e..5a4849ea7c 100644 --- a/APMrover2/failsafe.cpp +++ b/APMrover2/failsafe.cpp @@ -50,3 +50,13 @@ void Rover::failsafe_check() } } +#if ADVANCED_FAILSAFE == ENABLED +/* + check for AFS failsafe check + */ +void Rover::afs_fs_check(void) +{ + // perform AFS failsafe checks + g2.afs.check(rover.last_heartbeat_ms, false, failsafe.last_valid_rc_ms); // Rover don't have fence +} +#endif diff --git a/APMrover2/wscript b/APMrover2/wscript index 1c03981d69..af2d3a544e 100644 --- a/APMrover2/wscript +++ b/APMrover2/wscript @@ -21,6 +21,7 @@ def build(bld): 'PID', 'AP_Stats', 'AP_Beacon', + 'AP_AdvancedFailsafe', ], )