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

Copter: use mavlink fence message

Browse Source 
Note: we should consider moving this send via mavlink into the fence
library
This commit is contained in:
Randy Mackay 2013-05-01 17:06:58 +09:00
parent 2d17688363
commit 3aca35777f
2 changed files with 28 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
ArduCopter/ArduCopter.pde
View File

@ -821,7 +821,7 @@ static AP_Mount camera_mount2(&current_loc, g_gps, &ahrs, 1);
// AC_Fence library to reduce fly-aways
////////////////////////////////////////////////////////////////////////////////
#if AC_FENCE == ENABLED
AC_Fence fence(&inertial_nav, &g_gps);
AC_Fence fence(&inertial_nav);
#endif
////////////////////////////////////////////////////////////////////////////////

48
ArduCopter/fence.pde
View File

@ -18,8 +18,8 @@ void fence_check()
return;
}
// give fence library our current distance from home
fence.set_home_distance(home_distance);
// 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();
@ -30,23 +30,23 @@ void fence_check()
// 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
// 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 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 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
@ -69,18 +69,24 @@ void fence_check()
// 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());
// 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());
}
}