mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-05 07:28:29 -04:00
03831cdd28
Removed redundant checks to GPS_ok before setting flight mode to RTL (this check is already performed inside the set_mode function) Removed reset of home distance and bearing when GPS lock is lost, it now remains at the last known value
84 lines
3.0 KiB
Plaintext
84 lines
3.0 KiB
Plaintext
/// -*- 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 in meters
|
|
fence.set_home_distance(home_distance*0.01f);
|
|
|
|
// 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 if 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(manual_flight_mode(control_mode) && g.rc_3.control_in == 0 && !failsafe.radio && ((fence.get_breaches() & AC_FENCE_TYPE_ALT_MAX)== 0)){
|
|
init_disarm_motors();
|
|
}else{
|
|
// if we are within 100m of the fence, RTL
|
|
if (fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
|
|
if (!set_mode(RTL)) {
|
|
set_mode(LAND);
|
|
}
|
|
}else{
|
|
// if more than 100m outside the fence just force a 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()) {
|
|
// traslate fence library breach types to mavlink breach types
|
|
uint8_t mavlink_breach_type = FENCE_BREACH_NONE;
|
|
uint8_t breaches = fence.get_breaches();
|
|
if ((breaches & AC_FENCE_TYPE_ALT_MAX) != 0) {
|
|
mavlink_breach_type = FENCE_BREACH_MAXALT;
|
|
}
|
|
if ((breaches & AC_FENCE_TYPE_CIRCLE) != 0) {
|
|
mavlink_breach_type = FENCE_BREACH_BOUNDARY;
|
|
}
|
|
|
|
// send status
|
|
mavlink_msg_fence_status_send(chan,
|
|
(int8_t)(fence.get_breaches()!=0),
|
|
fence.get_breach_count(),
|
|
mavlink_breach_type,
|
|
fence.get_breach_time());
|
|
}
|
|
}
|
|
|
|
#endif
|