4 #if ADSB_ENABLED == ENABLED 15 MAV_COLLISION_ACTION actual_action = requested_action;
16 bool failsafe_state_change =
false;
19 if (!
copter.failsafe.adsb) {
20 copter.failsafe.adsb =
true;
21 failsafe_state_change =
true;
32 actual_action = MAV_COLLISION_ACTION_NONE;
36 if ((actual_action > MAV_COLLISION_ACTION_REPORT) &&
copter.should_disarm_on_failsafe()) {
37 copter.init_disarm_motors();
38 actual_action = MAV_COLLISION_ACTION_NONE;
42 switch (actual_action) {
44 case MAV_COLLISION_ACTION_RTL:
46 if (failsafe_state_change) {
48 actual_action = MAV_COLLISION_ACTION_NONE;
53 case MAV_COLLISION_ACTION_HOVER:
55 if (failsafe_state_change) {
57 actual_action = MAV_COLLISION_ACTION_NONE;
62 case MAV_COLLISION_ACTION_ASCEND_OR_DESCEND:
64 if (!handle_avoidance_vertical(obstacle, failsafe_state_change)) {
65 actual_action = MAV_COLLISION_ACTION_NONE;
69 case MAV_COLLISION_ACTION_MOVE_HORIZONTALLY:
71 if (!handle_avoidance_horizontal(obstacle, failsafe_state_change)) {
72 actual_action = MAV_COLLISION_ACTION_NONE;
76 case MAV_COLLISION_ACTION_MOVE_PERPENDICULAR:
77 if (!handle_avoidance_perpendicular(obstacle, failsafe_state_change)) {
78 actual_action = MAV_COLLISION_ACTION_NONE;
83 case MAV_COLLISION_ACTION_NONE:
85 case MAV_COLLISION_ACTION_REPORT:
92 if (failsafe_state_change) {
103 if (
copter.failsafe.adsb) {
104 copter.failsafe.adsb =
false;
109 switch (recovery_action) {
120 set_mode_else_try_RTL_else_LAND(
RTL);
125 set_mode_else_try_RTL_else_LAND(
AUTO);
164 if (!check_flightmode(allow_mode_change)) {
169 bool should_climb =
false;
171 if (_ahrs.get_position(my_loc)) {
178 velocity_neu.
z =
copter.wp_nav->get_speed_up();
180 velocity_neu.
z = -
copter.wp_nav->get_speed_down();
183 velocity_neu.
z = 0.0f;
188 copter.mode_avoid_adsb.set_velocity(velocity_neu);
195 if (!check_flightmode(allow_mode_change)) {
201 if (get_vector_perpendicular(obstacle, velocity_neu)) {
203 velocity_neu.
z = 0.0f;
211 velocity_neu.
x *=
copter.wp_nav->get_speed_xy();
212 velocity_neu.
y *=
copter.wp_nav->get_speed_xy();
214 copter.mode_avoid_adsb.set_velocity(velocity_neu);
225 if (!check_flightmode(allow_mode_change)) {
231 if (get_vector_perpendicular(obstacle, velocity_neu)) {
233 velocity_neu.
x *=
copter.wp_nav->get_speed_xy();
234 velocity_neu.
y *=
copter.wp_nav->get_speed_xy();
236 if (velocity_neu.
z > 0.0f) {
237 velocity_neu.
z *=
copter.wp_nav->get_speed_up();
239 velocity_neu.
z *=
copter.wp_nav->get_speed_down();
242 velocity_neu.
z = 0.0f;
246 copter.mode_avoid_adsb.set_velocity(velocity_neu);
void avoidance_adsb_update(void)
AP_Avoidance_Copter avoidance_adsb
void set_mode_else_try_RTL_else_LAND(control_mode_t mode)
bool check_flightmode(bool allow_mode_change)
control_mode_t prev_control_mode
bool handle_avoidance_perpendicular(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
#define AP_AVOIDANCE_RECOVERY_REMAIN_IN_AVOID_ADSB
#define MODE_THROW_ENABLED
bool is_zero(const T fVal1)
#define AP_AVOIDANCE_RECOVERY_RTL
bool handle_avoidance_vertical(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
AP_HAL_MAIN_CALLBACKS & copter
#define ERROR_SUBSYSTEM_FAILSAFE_ADSB
#define AP_AVOIDANCE_RECOVERY_RESUME_PREVIOUS_FLIGHTMODE
bool handle_avoidance_horizontal(const AP_Avoidance::Obstacle *obstacle, bool allow_mode_change)
#define ERROR_CODE_ERROR_RESOLVED
void handle_recovery(uint8_t recovery_action) override
MAV_COLLISION_ACTION handle_avoidance(const AP_Avoidance::Obstacle *obstacle, MAV_COLLISION_ACTION requested_action) override
#define AP_AVOIDANCE_RECOVERY_RESUME_IF_AUTO_ELSE_LOITER