Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-08 17:08:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Copter: integrate AC_Fence in place of limits

Browse Source 
Lesser functionality than limits but saves more than 150 bytes and it
works
This commit is contained in:
Randy Mackay 2013-04-26 18:51:07 +09:00
parent 9910d6d1cd
commit 58d1da2d80
11 changed files with 137 additions and 500 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduCopter/ArduCopter.pde
View File

@ -99,7 +99,7 @@
#include <AP_InertialNav.h> // ArduPilot Mega inertial navigation library
#include <AC_WPNav.h> // ArduCopter waypoint navigation library
#include <AP_Declination.h> // ArduPilot Mega Declination Helper Library
#include <AP_Limits.h>
#include <AC_Fence.h> // Arducopter Fence library
#include <memcheck.h> // memory limit checker
#include <SITL.h> // software in the loop support
#include <AP_Scheduler.h> // main loop scheduler
@ -819,15 +819,11 @@ static AP_Mount camera_mount(&current_loc, g_gps, &ahrs, 0);
static AP_Mount camera_mount2(&current_loc, g_gps, &ahrs, 1);
#endif
////////////////////////////////////////////////////////////////////////////////
// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters
// AC_Fence library to reduce fly-aways
////////////////////////////////////////////////////////////////////////////////
#if AP_LIMITS == ENABLED
AP_Limits limits;
AP_Limit_GPSLock gpslock_limit(g_gps);
AP_Limit_Geofence geofence_limit(FENCE_START_BYTE, FENCE_WP_SIZE, MAX_FENCEPOINTS, g_gps, &home, &current_loc);
AP_Limit_Altitude altitude_limit(&current_loc);
#if AC_FENCE == ENABLED
AC_Fence fence(&inertial_nav, &g_gps);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -1195,17 +1191,9 @@ static void fifty_hz_loop()
}
// slow_loop - 3.3hz loop
static void slow_loop()
{
#if AP_LIMITS == ENABLED
// Run the AP_Limits main loop
limits_loop();
#endif // AP_LIMITS_ENABLED
// This is the slow (3 1/3 Hz) loop pieces
//----------------------------------------
switch (slow_loopCounter) {
@ -1230,6 +1218,11 @@ static void slow_loop()
motors.set_frame_orientation(g.frame_orientation);
}
#if AC_FENCE == ENABLED
// check if we have breached a fence
fence_check();
#endif // AC_FENCE_ENABLED
break;
case 1:

17
ArduCopter/GCS_Mavlink.pde
View File

@ -111,10 +111,10 @@ static NOINLINE void send_attitude(mavlink_channel_t chan)
omega.z);
}
#if AP_LIMITS == ENABLED
#if AC_FENCE == ENABLED
static NOINLINE void send_limits_status(mavlink_channel_t chan)
{
limits_send_mavlink_status(chan);
fence_send_mavlink_status(chan);
}
#endif
@ -626,13 +626,11 @@ static bool mavlink_try_send_message(mavlink_channel_t chan, enum ap_message id,
send_statustext(chan);
break;
#if AP_LIMITS == ENABLED
#if AC_FENCE == ENABLED
case MSG_LIMITS_STATUS:
CHECK_PAYLOAD_SIZE(LIMITS_STATUS);
send_limits_status(chan);
break;
#endif
case MSG_AHRS:
@ -2084,8 +2082,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
break;
}
#if AP_LIMITS == ENABLED
/* To-Do: add back support for polygon type fence
#if AC_FENCE == ENABLED
// receive an AP_Limits fence point from GCS and store in EEPROM
// receive a fence point from GCS and store in EEPROM
case MAVLINK_MSG_ID_FENCE_POINT: {
@ -2116,9 +2114,8 @@ void GCS_MAVLINK::handleMessage(mavlink_message_t* msg)
}
break;
}
#endif // AP_LIMITS ENABLED
#endif // AC_FENCE ENABLED
*/
} // end switch
} // end handle mavlink

5
ArduCopter/Parameters.h
View File

@ -85,10 +85,7 @@ public:
k_param_sonar_gain, // 30
// 65: AP_Limits Library
k_param_limits = 65,
k_param_gpslock_limit,
k_param_geofence_limit,
k_param_altitude_limit,
k_param_fence = 65,
//
// 80: Heli

9
ArduCopter/Parameters.pde
View File

@ -932,13 +932,10 @@ const AP_Param::Info var_info[] PROGMEM = {
GOBJECT(barometer, "GND_", AP_Baro),
GOBJECT(scheduler, "SCHED_", AP_Scheduler),
#if AP_LIMITS == ENABLED
#if AC_FENCE == ENABLED
//@Group: LIM_
//@Path: ../libraries/AP_Limits/AP_Limits.cpp,../libraries/AP_Limits/AP_Limit_GPSLock.cpp,../libraries/AP_Limits/AP_Limit_Geofence.cpp,../libraries/AP_Limits/AP_Limit_Altitude.cpp,../libraries/AP_Limits/AP_Limit_Module.cpp
GOBJECT(limits, "LIM_", AP_Limits),
GOBJECT(gpslock_limit, "LIM_", AP_Limit_GPSLock),
GOBJECT(geofence_limit, "LIM_", AP_Limit_Geofence),
GOBJECT(altitude_limit, "LIM_", AP_Limit_Altitude),
//@Path: ../libraries/AC_Fence/AC_Fence.cpp
GOBJECT(fence, "FENCE_", AC_Fence),
#endif
#if FRAME_CONFIG == HELI_FRAME

31
ArduCopter/config.h
View File

@ -1062,37 +1062,10 @@
//
// Enable/disable AP_Limits
#ifndef AP_LIMITS
#define AP_LIMITS ENABLED
#ifndef AC_FENCE
#define AC_FENCE ENABLED
#endif
// Use PIN for displaying LIMITS status. 0 is disabled.
#ifndef LIMITS_TRIGGERED_PIN
#define LIMITS_TRIGGERED_PIN 0
#endif
// PWM of "on" state for LIM_CHANNEL
#ifndef LIMITS_ENABLE_PWM
#define LIMITS_ENABLE_PWM 1800
#endif
#ifndef LIM_ENABLED
#define LIM_ENABLED 0
#endif
#ifndef LIM_ALT_ON
#define LIM_ALT_ON 0
#endif
#ifndef LIM_FNC_ON
#define LIM_FNC_ON 0
#endif
#ifndef LIM_GPSLCK_ON
#define LIM_GPSLCK_ON 0
#endif
//////////////////////////////////////////////////////////////////////////////
// Developer Items

7
ArduCopter/control_modes.pde
View File

@ -169,6 +169,13 @@ static void read_trim_switch()
// enable or disable the sonar
g.sonar_enabled = ap_system.CH7_flag;
break;
#if AC_FENCE == ENABLED
case CH7_FENCE:
// enable or disable the fence
fence.enable(ap_system.CH7_flag);
break;
#endif
}
}

24
ArduCopter/defines.h
View File

@ -49,17 +49,18 @@
#define CH6_PWM_TRIGGER_HIGH 1800
#define CH6_PWM_TRIGGER_LOW 1200
#define CH7_DO_NOTHING 0
#define CH7_SET_HOVER 1 // deprecated
#define CH7_FLIP 2
#define CH7_SIMPLE_MODE 3
#define CH7_RTL 4
#define CH7_SAVE_TRIM 5
#define CH7_ADC_FILTER 6 // deprecated
#define CH7_SAVE_WP 7
#define CH7_MULTI_MODE 8
#define CH7_CAMERA_TRIGGER 9
#define CH7_SONAR 10 // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground
#define CH7_DO_NOTHING 0
#define CH7_SET_HOVER 1 // deprecated
#define CH7_FLIP 2
#define CH7_SIMPLE_MODE 3
#define CH7_RTL 4
#define CH7_SAVE_TRIM 5
#define CH7_ADC_FILTER 6 // deprecated
#define CH7_SAVE_WP 7
#define CH7_MULTI_MODE 8
#define CH7_CAMERA_TRIGGER 9
#define CH7_SONAR 10 // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground
#define CH7_FENCE 11 // allow enabling or disabling fence in flight
@ -434,6 +435,7 @@ enum gcs_severity {
#define ERROR_SUBSYSTEM_FAILSAFE_RADIO 5
#define ERROR_SUBSYSTEM_FAILSAFE_BATT 6
#define ERROR_SUBSYSTEM_FAILSAFE_GPS 7
#define ERROR_SUBSYSTEM_FAILSAFE_FENCE 8
// general error codes
#define ERROR_CODE_ERROR_RESOLVED 0
#define ERROR_CODE_FAILED_TO_INITIALISE 1

87
ArduCopter/fence.pde Normal file
View File

@ -0,0 +1,87 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
// Code to integrate AC_Fence library with main ArduCopter code
#if AC_FENCE == ENABLED
uint8_t lim_state = 0, lim_old_state = 0;
// fence_check - ask fence library to check for breaches and initiate the response
// called at 1hz
void fence_check()
{
uint8_t new_breaches; // the type of fence that has been breached
uint8_t orig_breaches = fence.get_breaches();
// return immediately if motors are not armed
if(!motors.armed()) {
return;
}
// give fence library our current distance from home
fence.set_home_distance(home_distance);
// check for a breach
new_breaches = fence.check_fence();
// if there is a new breach take action
if( new_breaches != AC_FENCE_TYPE_NONE ) {
// if the user wants some kind of response and motors are armed
if(fence.get_action() != AC_FENCE_ACTION_REPORT_ONLY ) {
// disarm immediately we think we are on the ground
// don't disarm if the high-altitude fence has been broken because it's likely the user has pulled their throttle to zero to bring it down
if(control_mode <= ACRO && g.rc_3.control_in == 0 && !ap.failsafe_radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
init_disarm_motors();
}else{
// if we have a GPS
if( ap.home_is_set ) {
// if the breach is of the big circle LAND
if((new_breaches & AC_FENCE_TYPE_BIG_CIRCLE) > 0) {
if(control_mode != LAND) {
set_mode(LAND);
}
}else{
// must be a small circle or altitude breach so try to RTL
if(control_mode != RTL) {
set_mode(RTL);
}
}
}else{
// we have no GPS so LAND
if(control_mode != LAND) {
set_mode(LAND);
}
}
}
}
// log an error in the dataflash
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, new_breaches);
}
// record clearing of breach
if(orig_breaches != AC_FENCE_TYPE_NONE && fence.get_breaches() == AC_FENCE_TYPE_NONE) {
Log_Write_Error(ERROR_SUBSYSTEM_FAILSAFE_FENCE, ERROR_CODE_ERROR_RESOLVED);
}
}
// fence_send_mavlink_status - send fence status to ground station
static void fence_send_mavlink_status(mavlink_channel_t chan)
{
if (fence.enabled()) {
mavlink_msg_limits_status_send(chan,
(uint8_t) fence.enabled(),
(uint32_t) fence.get_breach_time(),
(uint32_t) 0,
(uint32_t) 0,
(uint32_t) 0,
(uint16_t) fence.get_breach_count(),
(uint8_t) fence.get_enabled_fences(),
(uint8_t) 0,
(uint8_t) fence.get_breaches());
}
}
#endif

366
ArduCopter/limits.pde
View File

@ -1,366 +0,0 @@
// Main state machine loop for AP_Limits. Called from slow or superslow loop.
#if AP_LIMITS == ENABLED
uint8_t lim_state = 0, lim_old_state = 0;
void set_recovery_home_alt() {
uint32_t return_altitude_cm_ahl = 0; // in centimeters above home level.
uint32_t amin_meters_ahl, amax_meters_ahl;
// for flying vehicles only
if (altitude_limit.enabled()) {
amin_meters_ahl = (uint32_t) (altitude_limit.min_alt());
amax_meters_ahl = (uint32_t) (altitude_limit.max_alt());
// See if we have a meaningful setting
if (amax_meters_ahl && ((amax_meters_ahl - amin_meters_ahl) > 1)) {
// there is a max_alt set
// set a return altitude that is halfway between the minimum and maximum altitude setting.
// return_altitude is in centimeters, not meters, so we multiply
return_altitude_cm_ahl = (uint32_t) (home.alt + (100 * (uint16_t) ((amax_meters_ahl - amin_meters_ahl) / 2)));
}
} else {
return_altitude_cm_ahl = (uint32_t) (home.alt + g.rtl_altitude);
}
// final sanity check
// if our return is less than 4 meters from ground, set it to 4m, to clear "people" height.
if ((return_altitude_cm_ahl - (uint32_t) home.alt) < 400) {
return_altitude_cm_ahl = home.alt + 400;
}
// To-Do: ensure target is fed into wp_nav controller. Note this function is currently not called
//guided_WP.id = 0;
//guided_WP.p1 = 0;
//guided_WP.options = 0;
//guided_WP.lat = home.lat;
//guided_WP.lng = home.lng;
//guided_WP.alt = return_altitude_cm_ahl;
}
static void limits_loop() {
lim_state = limits.state();
// Use limits channel to determine LIMITS_ENABLED or LIMITS_DISABLED state
if (lim_state != LIMITS_DISABLED && limits.channel() !=0
&& hal.rcin->read(limits.channel()-1) < LIMITS_ENABLE_PWM) {
limits.set_state(LIMITS_DISABLED);
}
else if (lim_state == LIMITS_DISABLED && limits.channel() !=0
&& hal.rcin->read(limits.channel()-1) >= LIMITS_ENABLE_PWM) {
limits.set_state(LIMITS_ENABLED);
}
if ((uint32_t) millis() - (uint32_t) limits.last_status_update > 1000) { // more than a second has passed - time for an update
gcs_send_message(MSG_LIMITS_STATUS);
}
if (lim_state != lim_old_state) { // state changed
lim_old_state = lim_state; // we only use lim_oldstate here, for reporting purposes. So, reset it.
gcs_send_message(MSG_LIMITS_STATUS);
if (limits.debug()) switch (lim_state) {
case LIMITS_INIT: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: INIT")); break;
case LIMITS_DISABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: DISABLED")); break;
case LIMITS_ENABLED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: ENABLED")); break;
case LIMITS_TRIGGERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: TRIGGERED")); break;
case LIMITS_RECOVERING: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERING")); break;
case LIMITS_RECOVERED: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: RECOVERED")); break;
default: gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - State change: UNKNOWN")); break;
}
}
switch (limits.state()) {
// have not initialized yet
case LIMITS_INIT:
if (limits.init()) { // initialize system
// See what the "master" on/off swith is and go to the appropriate start state
if (!limits.enabled()) {
limits.set_state(LIMITS_DISABLED);
}
else {
limits.set_state(LIMITS_ENABLED);
}
}
break;
// We have been switched off
case LIMITS_DISABLED:
// check if we have been switched on
if (limits.enabled()) {
limits.set_state(LIMITS_ENABLED);
break;
}
break;
// Limits module is enabled
case LIMITS_ENABLED:
// check if we've been switched off
if (!limits.enabled()) {
limits.set_state(LIMITS_DISABLED);
break;
}
// Until motors are armed, do nothing, just wait in ENABLED state
if (!motors.armed()) {
// we are waiting for motors to arm
// do nothing
break;
}
bool required_only;
required_only = (limits.last_clear == 0); // if we haven't yet 'cleared' all limits, check required limits only
// check if any limits have been breached and trigger if they have
if (limits.check_triggered(required_only)) {
//
// TRIGGER - BREACH OF LIMITS
//
// make a note of which limits triggered, so if we know if we recovered them
limits.mods_recovering = limits.mods_triggered;
limits.last_action = 0;
limits.last_trigger = millis();
limits.breach_count++;
limits.set_state(LIMITS_TRIGGERED);
break;
}
if (motors.armed() && limits.enabled() && !limits.mods_triggered) {
// All clear.
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - All Clear"));
limits.last_clear = millis();
}
break;
// Limits have been triggered
case LIMITS_TRIGGERED:
// check if we've been switched off
if (!limits.enabled()) {
limits.set_state(LIMITS_DISABLED);
break;
}
#if LIMITS_TRIGGERED_PIN > 0
digitalWrite(LIMITS_TRIGGERED_PIN, HIGH);
#endif
if (limits.debug()) {
if (limits.mods_triggered & LIMIT_GPSLOCK) gcs_send_text_P(SEVERITY_LOW, PSTR("!GPSLock"));
if (limits.mods_triggered & LIMIT_GEOFENCE) gcs_send_text_P(SEVERITY_LOW, PSTR("!Geofence"));
if (limits.mods_triggered & LIMIT_ALTITUDE) gcs_send_text_P(SEVERITY_LOW, PSTR("!Altitude"));
}
// If the motors are not armed, we have triggered pre-arm checks. Do nothing
if (motors.armed() == false) {
limits.set_state(LIMITS_ENABLED); // go back to checking limits
break;
}
// If we are triggered but no longer in breach, that means we recovered
// somehow, via auto recovery or pilot action
if (!limits.check_all()) {
limits.last_recovery = millis();
limits.set_state(LIMITS_RECOVERED);
break;
}
else {
limits.set_state(LIMITS_RECOVERING);
limits.last_action = 0; // reset timer
// We are about to take action on a real breach. Make sure we notify immediately
gcs_send_message(MSG_LIMITS_STATUS);
break;
}
break;
// Take action to recover
case LIMITS_RECOVERING:
// If the motors are not armed, we have triggered pre-arm checks. Do nothing
if (motors.armed() == false) {
limits.set_state(LIMITS_ENABLED); // go back to checking limits
break;
}
// check if we've been switched off
if (!limits.enabled() && limits.old_mode_switch == oldSwitchPosition) {
limits.old_mode_switch = 0;
reset_control_switch();
limits.set_state(LIMITS_DISABLED);
break;
}
// Still need action?
if (limits.check_all() == 0) { // all triggers clear
limits.set_state(LIMITS_RECOVERED);
break;
}
if (limits.mods_triggered != limits.mods_recovering) { // if any *new* triggers, hit the trigger again
//
// TRIGGER - BREACH OF LIMITS
//
// make a note of which limits triggered, so if we know if we recovered them
limits.mods_recovering = limits.mods_triggered;
limits.last_action = 0;
limits.last_trigger = millis();
limits.breach_count++;
limits.set_state(LIMITS_TRIGGERED);
limits.set_state(LIMITS_TRIGGERED);
break;
}
// Recovery Action
// if there was no previous action, take action, take note of time send GCS.
if (limits.last_action == 0) {
// save mode switch
limits.old_mode_switch = oldSwitchPosition;
// // Take action
// // This ensures no "radical" RTL, like a full throttle take-off,happens if something triggers at ground level
// if ((uint32_t) current_loc.alt < ((uint32_t)home.alt * 200) ) { // we're under 2m (200cm), already at "people" height or on the ground
// if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action: near ground - do nothing"));
// // TODO: Will this work for a plane? Does it make sense in general?
//
// //set_mode(LAND);
// limits.last_action = millis(); // start counter
// gcs_send_message(MSG_LIMITS_STATUS);
//
// break;
// }
// TODO: This applies only to planes - hold for porting
// if (control_mode == MANUAL && g.auto_trim) {
// // make sure we don't auto trim the surfaces on this change
// control_mode = STABILIZE;
// }
switch (limits.recmode()) {
case 0: // RTL mode
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - RTL"));
set_mode(RTL);
limits.last_action = millis();
gcs_send_message(MSG_LIMITS_STATUS);
break;
case 1: // Bounce mode
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Action - bounce mode, POSITION"));
// ALT_HOLD gives us yaw hold, roll& pitch hold and throttle hold.
// It is like position hold, but without manual throttle control.
//set_recovery_home_alt();
set_mode(POSITION);
set_throttle_mode(THROTTLE_AUTO);
limits.last_action = millis();
gcs_send_message(MSG_LIMITS_STATUS);
break;
}
break;
}
// In bounce mode, take control for 3 seconds, and then wait for the pilot to make us "safe".
// If the vehicle does not recover, the escalation action will trigger.
if (limits.recmode() == 1) {
if (control_mode == POSITION && ((uint32_t)millis() - (uint32_t)limits.last_action) > 3000) {
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Bounce: Returning control to pilot"));
set_mode(STABILIZE);
} else if (control_mode == STABILIZE && ((uint32_t)millis() - (uint32_t)limits.last_action) > 6000) {
// after 3 more seconds, reset action counter to take action again
limits.last_action = 0;
}
}
// ESCALATE We have not recovered after 2 minutes of recovery action
if (((uint32_t)millis() - (uint32_t)limits.last_action) > 120000 ) {
// TODO: Secondary recovery
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Recovery Escalation: RTL"));
set_mode(RTL);
limits.last_action = millis();
break;
}
break;
// Have recovered, relinquish control and re-enable
case LIMITS_RECOVERED:
// check if we've been switched off
if (!limits.enabled()) {
limits.set_state(LIMITS_DISABLED);
break;
}
#if LIMITS_TRIGGERED_PIN > 0
digitalWrite(LIMITS_TRIGGERED_PIN, LOW);
#endif
// Reset action counter
limits.last_action = 0;
if (((uint32_t)millis() - (uint32_t)limits.last_recovery) > (uint32_t)(limits.safetime() * 1000)) { // Wait "safetime" seconds of recovery before we give back control
// Our recovery action worked.
limits.set_state(LIMITS_ENABLED);
// Switch to stabilize
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits - Returning controls"));
set_mode(STABILIZE); limits.last_recovery = millis();
break;
}
break;
default:
if (limits.debug()) gcs_send_text_P(SEVERITY_LOW,PSTR("Limits: unknown state"));
break;
}
}
// This function below, should really be in the AP_Limits class, but it is impossible to untangle the mavlink includes.
void limits_send_mavlink_status(mavlink_channel_t chan) {
limits.last_status_update = millis();
if (limits.enabled()) {
mavlink_msg_limits_status_send(chan,
(uint8_t) limits.state(),
(uint32_t) limits.last_trigger,
(uint32_t) limits.last_action,
(uint32_t) limits.last_recovery,
(uint32_t) limits.last_clear,
(uint16_t) limits.breach_count,
(LimitModuleBits) limits.mods_enabled,
(LimitModuleBits) limits.mods_required,
(LimitModuleBits) limits.mods_triggered);
}
}
#endif

29
ArduCopter/motors.pde
View File

@ -51,29 +51,7 @@ static void arm_motors()
// arm the motors and configure for flight
if (arming_counter == ARM_DELAY && !motors.armed()) {
////////////////////////////////////////////////////////////////////////////////
// Experimental AP_Limits library - set constraints, limits, fences, minima, maxima on various parameters
////////////////////////////////////////////////////////////////////////////////
#if AP_LIMITS == ENABLED
if (limits.enabled() && limits.required()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("Limits - Running pre-arm checks"));
// check only pre-arm required modules
if (limits.check_required()) {
gcs_send_text_P(SEVERITY_LOW, PSTR("ARMING PREVENTED - Limit Breached"));
limits.set_state(LIMITS_TRIGGERED);
gcs_send_message(MSG_LIMITS_STATUS);
arming_counter++; // restart timer by cycling
}else{
init_arm_motors();
}
}else{
init_arm_motors();
}
#else // without AP_LIMITS, just arm motors
init_arm_motors();
#endif //AP_LIMITS_ENABLED
}
// arm the motors and configure for flight
@ -190,6 +168,13 @@ static void pre_arm_checks()
return;
}
#if AC_FENCE == ENABLED
// check fence is initialised
if(!fence.pre_arm_check()) {
return;
}
#endif
// if we've gotten this far then pre arm checks have completed
ap.pre_arm_check = true;
}

35
ArduCopter/system.pde
View File

@ -272,45 +272,10 @@ init_rate_controllers();
Log_Write_Startup();
#endif
init_ap_limits();
cliSerial->print_P(PSTR("\nReady to FLY "));
}
///////////////////////////////////////////////////////////////////////////////
// Experimental AP_Limits library - set constraints, limits, fences, minima,
// maxima on various parameters
////////////////////////////////////////////////////////////////////////////////
static void init_ap_limits() {
#if AP_LIMITS == ENABLED
// The linked list looks (logically) like this [limits module] -> [first
// limit module] -> [second limit module] -> [third limit module] -> NULL
// The details of the linked list are handled by the methods
// modules_first, modules_current, modules_next, modules_last, modules_add
// in limits
limits.modules_add(&gpslock_limit);
limits.modules_add(&geofence_limit);
limits.modules_add(&altitude_limit);
if (limits.debug()) {
gcs_send_text_P(SEVERITY_LOW,PSTR("Limits Modules Loaded"));
AP_Limit_Module *m = limits.modules_first();
while (m) {
gcs_send_text_P(SEVERITY_LOW, get_module_name(m->get_module_id()));
m = limits.modules_next();
}
}
#endif
}
//******************************************************************************
//This function does all the calibrations, etc. that we need during a ground start
//******************************************************************************