mirror of https://github.com/ArduPilot/ardupilot
94 lines
3.4 KiB
Plaintext
94 lines
3.4 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(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 we are within 100m of the fence, RTL
|
|
if( fence.get_breach_distance(new_breaches) <= AC_FENCE_GIVE_UP_DISTANCE) {
|
|
if(control_mode != RTL) {
|
|
set_mode(RTL);
|
|
}
|
|
}else{
|
|
// if more than 100m outside the fence just force a land
|
|
if(control_mode != RTL) {
|
|
set_mode(LAND);
|
|
}
|
|
}
|
|
}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()) {
|
|
// 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
|