Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
A
ardupilot
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Terms and privacy
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
SASC
ardupilot
Commits
2da33e41
"...build/classes/.netbeans_update_resources" did not exist on "9a6d8f667bf763f2f23960983a8356e1d90a3c60"
Commit
2da33e41
authored
8 years ago
by
Tom Pittenger
Browse files
Options
Downloads
Patches
Plain Diff
Plane: add Avoid_ADSB support to plane
parent
330c63b0
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
ArduPlane/avoidance_adsb.cpp
+167
-4
167 additions, 4 deletions
ArduPlane/avoidance_adsb.cpp
ArduPlane/avoidance_adsb.h
+0
-3
0 additions, 3 deletions
ArduPlane/avoidance_adsb.h
with
167 additions
and
7 deletions
ArduPlane/avoidance_adsb.cpp
+
167
−
4
View file @
2da33e41
...
...
@@ -12,6 +12,86 @@ void Plane::avoidance_adsb_update(void)
MAV_COLLISION_ACTION
AP_Avoidance_Plane
::
handle_avoidance
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
MAV_COLLISION_ACTION
requested_action
)
{
MAV_COLLISION_ACTION
actual_action
=
requested_action
;
bool
failsafe_state_change
=
false
;
// check for changes in failsafe
if
(
!
plane
.
failsafe
.
adsb
)
{
plane
.
failsafe
.
adsb
=
true
;
failsafe_state_change
=
true
;
// record flight mode in case it's required for the recovery
prev_control_mode
=
plane
.
control_mode
;
}
// take no action in some flight modes
if
(
plane
.
control_mode
==
MANUAL
||
(
plane
.
control_mode
==
AUTO
&&
!
plane
.
auto_state
.
takeoff_complete
)
||
(
plane
.
control_mode
==
AUTO
&&
plane
.
auto_state
.
land_in_progress
)
||
// TODO: consider allowing action during approach
plane
.
control_mode
==
AUTOTUNE
||
plane
.
control_mode
==
QLAND
)
{
actual_action
=
MAV_COLLISION_ACTION_NONE
;
}
// take action based on requested action
switch
(
actual_action
)
{
case
MAV_COLLISION_ACTION_RTL
:
if
(
failsafe_state_change
)
{
plane
.
set_mode
(
RTL
,
MODE_REASON_AVOIDANCE
);
}
break
;
case
MAV_COLLISION_ACTION_HOVER
:
if
(
failsafe_state_change
)
{
if
(
plane
.
quadplane
.
is_flying
())
{
plane
.
set_mode
(
QLOITER
,
MODE_REASON_AVOIDANCE
);
}
else
{
plane
.
set_mode
(
LOITER
,
MODE_REASON_AVOIDANCE
);
}
}
break
;
case
MAV_COLLISION_ACTION_ASCEND_OR_DESCEND
:
// climb or descend to avoid obstacle
if
(
handle_avoidance_vertical
(
obstacle
,
failsafe_state_change
))
{
plane
.
set_guided_WP
();
}
else
{
actual_action
=
MAV_COLLISION_ACTION_NONE
;
}
break
;
case
MAV_COLLISION_ACTION_MOVE_HORIZONTALLY
:
// move horizontally to avoid obstacle
if
(
handle_avoidance_horizontal
(
obstacle
,
failsafe_state_change
))
{
plane
.
set_guided_WP
();
}
else
{
actual_action
=
MAV_COLLISION_ACTION_NONE
;
}
break
;
case
MAV_COLLISION_ACTION_MOVE_PERPENDICULAR
:
{
// move horizontally and vertically to avoid obstacle
const
bool
success_vert
=
handle_avoidance_vertical
(
obstacle
,
failsafe_state_change
);
const
bool
success_hor
=
handle_avoidance_horizontal
(
obstacle
,
failsafe_state_change
);
if
(
success_vert
||
success_hor
)
{
plane
.
set_guided_WP
();
}
else
{
actual_action
=
MAV_COLLISION_ACTION_NONE
;
}
}
break
;
// unsupported actions and those that require no response
case
MAV_COLLISION_ACTION_NONE
:
return
actual_action
;
case
MAV_COLLISION_ACTION_REPORT
:
default
:
break
;
}
if
(
failsafe_state_change
)
{
GCS_MAVLINK
::
send_statustext_all
(
MAV_SEVERITY_ALERT
,
"Avoid: Performing action: %d"
,
actual_action
);
}
// return with action taken
return
actual_action
;
...
...
@@ -19,23 +99,106 @@ MAV_COLLISION_ACTION AP_Avoidance_Plane::handle_avoidance(const AP_Avoidance::Ob
void
AP_Avoidance_Plane
::
handle_recovery
(
uint8_t
recovery_action
)
{
// check we are coming out of failsafe
if
(
plane
.
failsafe
.
adsb
)
{
plane
.
failsafe
.
adsb
=
false
;
GCS_MAVLINK
::
send_statustext_all
(
MAV_SEVERITY_INFO
,
"Avoid: Resuming with action: %d"
,
recovery_action
);
// restore flight mode if requested and user has not changed mode since
if
(
plane
.
control_mode_reason
==
MODE_REASON_AVOIDANCE
)
{
switch
(
recovery_action
)
{
case
AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB
:
// do nothing, we'll stay in the AVOID_ADSB mode which is guided which will loiter
break
;
case
AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE
:
plane
.
set_mode
(
prev_control_mode
,
MODE_REASON_AVOIDANCE_RECOVERY
);
break
;
case
AP_AVOIDANCE_RECOVERY_RTL
:
plane
.
set_mode
(
RTL
,
MODE_REASON_AVOIDANCE_RECOVERY
);
break
;
case
AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER
:
if
(
prev_control_mode
==
AUTO
)
{
plane
.
set_mode
(
AUTO
,
MODE_REASON_AVOIDANCE_RECOVERY
);
}
// else do nothing, same as AP_AVOIDANCE_RECOVERY_LOITER
break
;
default
:
break
;
}
// switch
}
}
}
// check flight mode is avoid_adsb
bool
AP_Avoidance_Plane
::
check_flightmode
(
bool
allow_mode_change
)
{
// ensure plane is in avoid_adsb mode
if
(
allow_mode_change
&&
plane
.
control_mode
!=
AVOID_ADSB
)
{
plane
.
set_mode
(
AVOID_ADSB
,
MODE_REASON_AVOIDANCE
);
}
// check flight mode
return
(
plane
.
control_mode
==
AVOID_ADSB
);
}
bool
AP_Avoidance_Plane
::
handle_avoidance_
horizont
al
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
bool
allow_mode_change
)
bool
AP_Avoidance_Plane
::
handle_avoidance_
vertic
al
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
bool
allow_mode_change
)
{
// if we got this far we failed to set the new target
return
false
;
// ensure copter is in avoid_adsb mode
if
(
!
check_flightmode
(
allow_mode_change
))
{
return
false
;
}
// get best vector away from obstacle
if
(
plane
.
current_loc
.
alt
>
obstacle
->
_location
.
alt
)
{
// should climb
plane
.
guided_WP_loc
.
alt
=
plane
.
current_loc
.
alt
+
1000
;
// set alt demand to be 10m above us, climb rate will be TECS_CLMB_MAX
return
true
;
}
else
if
(
plane
.
current_loc
.
alt
>
plane
.
g
.
RTL_altitude_cm
)
{
// should descend while above RTL alt
// TODO: consider using a lower altitude than RTL_altitude_cm since it's default (100m) is quite high
plane
.
guided_WP_loc
.
alt
=
plane
.
current_loc
.
alt
-
1000
;
// set alt demand to be 10m below us, sink rate will be TECS_SINK_MAX
return
true
;
}
return
false
;
}
bool
AP_Avoidance_Plane
::
handle_avoidance_
perpendicular
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
bool
allow_mode_change
)
bool
AP_Avoidance_Plane
::
handle_avoidance_
horizontal
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
bool
allow_mode_change
)
{
// ensure plane is in avoid_adsb mode
if
(
!
check_flightmode
(
allow_mode_change
))
{
return
false
;
}
// get best vector away from obstacle
Vector3f
velocity_neu
;
if
(
get_vector_perpendicular
(
obstacle
,
velocity_neu
))
{
// remove vertical component
velocity_neu
.
z
=
0.0
f
;
// check for divide by zero
if
(
is_zero
(
velocity_neu
.
x
)
&&
is_zero
(
velocity_neu
.
y
))
{
return
false
;
}
// re-normalize
velocity_neu
.
normalize
();
// push vector further away.
velocity_neu
*=
10000
;
// set target
location_offset
(
plane
.
guided_WP_loc
,
velocity_neu
.
x
,
velocity_neu
.
y
);
return
true
;
}
// if we got this far we failed to set the new target
return
false
;
}
This diff is collapsed.
Click to expand it.
ArduPlane/avoidance_adsb.h
+
0
−
3
View file @
2da33e41
...
...
@@ -30,9 +30,6 @@ protected:
// horizontal avoidance handler
bool
handle_avoidance_horizontal
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
bool
allow_mode_change
);
// perpendicular (3 dimensional) avoidance handler
bool
handle_avoidance_perpendicular
(
const
AP_Avoidance
::
Obstacle
*
obstacle
,
bool
allow_mode_change
);
// control mode before avoidance began
FlightMode
prev_control_mode
=
RTL
;
};
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment