From 58d1da2d809a757ee7203ab24836d3265d0e43a4 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 26 Apr 2013 18:51:07 +0900 Subject: [PATCH] Copter: integrate AC_Fence in place of limits Lesser functionality than limits but saves more than 150 bytes and it works --- ArduCopter/ArduCopter.pde | 27 +-- ArduCopter/GCS_Mavlink.pde | 17 +- ArduCopter/Parameters.h | 5 +- ArduCopter/Parameters.pde | 9 +- ArduCopter/config.h | 31 +-- ArduCopter/control_modes.pde | 7 + ArduCopter/defines.h | 24 +-- ArduCopter/fence.pde | 87 +++++++++ ArduCopter/limits.pde | 366 ----------------------------------- ArduCopter/motors.pde | 29 +-- ArduCopter/system.pde | 35 ---- 11 files changed, 137 insertions(+), 500 deletions(-) create mode 100644 ArduCopter/fence.pde delete mode 100644 ArduCopter/limits.pde diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index b941259de1..1316eba993 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -99,7 +99,7 @@ #include // ArduPilot Mega inertial navigation library #include // ArduCopter waypoint navigation library #include // ArduPilot Mega Declination Helper Library -#include +#include // Arducopter Fence library #include // memory limit checker #include // software in the loop support #include // main loop scheduler @@ -819,15 +819,11 @@ static AP_Mount camera_mount(¤t_loc, g_gps, &ahrs, 0); static AP_Mount camera_mount2(¤t_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, ¤t_loc); -AP_Limit_Altitude altitude_limit(¤t_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: diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 786aa58890..8787dc3c85 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -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 diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index ed0da105c5..49420db29c 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -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 diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index 7d198b7ad7..e151664ea0 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -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 diff --git a/ArduCopter/config.h b/ArduCopter/config.h index c806ee82c3..2b2ccdb6cc 100644 --- a/ArduCopter/config.h +++ b/ArduCopter/config.h @@ -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 diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index d31603a34a..362f661a07 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -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 } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index be847d5b43..9ffc770756 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -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 diff --git a/ArduCopter/fence.pde b/ArduCopter/fence.pde new file mode 100644 index 0000000000..7f0587b52d --- /dev/null +++ b/ArduCopter/fence.pde @@ -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 diff --git a/ArduCopter/limits.pde b/ArduCopter/limits.pde deleted file mode 100644 index 6f3004f5b2..0000000000 --- a/ArduCopter/limits.pde +++ /dev/null @@ -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 diff --git a/ArduCopter/motors.pde b/ArduCopter/motors.pde index b930467981..bf57fd7dc7 100644 --- a/ArduCopter/motors.pde +++ b/ArduCopter/motors.pde @@ -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; } diff --git a/ArduCopter/system.pde b/ArduCopter/system.pde index 738af97b01..8f20a04527 100644 --- a/ArduCopter/system.pde +++ b/ArduCopter/system.pde @@ -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 //******************************************************************************