mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-06 07:58:28 -04:00
7ea971d948
Previously if set_mode failed it would return the copter to stabilize mode. With this commit set_mode returns a true/false indicating whether it succeeded or not so the caller can make the decision as to the appropriate response which could be to stay in the current flight mode or try another flight mode.
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 (GPS_ok()) {
|
|
// 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 != LAND) {
|
|
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
|