Browse Source

Copter: obey ADSB_ENABLED == DISABLED

master
Dr.-Ing. Amilcar Do Carmo Lucas 7 years ago committed by Randy Mackay
parent
commit
def098bd8a
  1. 4
      ArduCopter/AP_Arming.cpp
  2. 2
      ArduCopter/ArduCopter.cpp
  3. 8
      ArduCopter/Copter.h
  4. 4
      ArduCopter/GCS_Mavlink.cpp
  5. 2
      ArduCopter/Parameters.cpp
  6. 2
      ArduCopter/avoidance_adsb.cpp
  7. 4
      ArduCopter/mode.cpp
  8. 2
      ArduCopter/switches.cpp

4
ArduCopter/AP_Arming.cpp

@ -242,12 +242,14 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure) @@ -242,12 +242,14 @@ bool AP_Arming_Copter::parameter_checks(bool display_failure)
}
// check adsb avoidance failsafe
#if ADSB_ENABLED == ENABLE
if (copter.failsafe.adsb) {
if (display_failure) {
gcs().send_text(MAV_SEVERITY_CRITICAL,"PreArm: ADSB threat detected");
}
return false;
}
#endif
// check for something close to vehicle
if (!pre_arm_proximity_check(display_failure)) {
@ -630,6 +632,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) @@ -630,6 +632,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
}
// check adsb
#if ADSB_ENABLED == ENABLE
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_PARAMETERS)) {
if (copter.failsafe.adsb) {
if (display_failure) {
@ -638,6 +641,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs) @@ -638,6 +641,7 @@ bool AP_Arming_Copter::arm_checks(bool display_failure, bool arming_from_gcs)
return false;
}
}
#endif
// check throttle
if ((checks_to_perform == ARMING_CHECK_ALL) || (checks_to_perform & ARMING_CHECK_RC)) {

2
ArduCopter/ArduCopter.cpp

@ -422,7 +422,9 @@ void Copter::one_hz_loop() @@ -422,7 +422,9 @@ void Copter::one_hz_loop()
// log terrain data
terrain_logging();
#if ADSB_ENABLED == ENABLED
adsb.set_is_flying(!ap.land_complete);
#endif
// update error mask of sensors and subsystems. The mask uses the
// MAV_SYS_STATUS_* values from mavlink. If a bit is set then it

8
ArduCopter/Copter.h

@ -151,7 +151,9 @@ @@ -151,7 +151,9 @@
// Local modules
#include "Parameters.h"
#if ADSB_ENABLED == ENABLED
#include "avoidance_adsb.h"
#endif
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
#include <SITL/SITL.h>
@ -552,10 +554,12 @@ private: @@ -552,10 +554,12 @@ private:
AC_InputManager_Heli input_manager;
#endif
#if ADSB_ENABLED == ENABLED
AP_ADSB adsb{ahrs};
// avoidance of adsb enabled vehicles (normally manned vehicles)
AP_Avoidance_Copter avoidance_adsb{ahrs, adsb};
#endif
// use this to prevent recursion during sensor init
bool in_mavlink_delay;
@ -652,8 +656,10 @@ private: @@ -652,8 +656,10 @@ private:
void rotate_body_frame_to_NE(float &x, float &y);
uint16_t get_pilot_speed_dn();
#if ADSB_ENABLED == ENABLED
// avoidance_adsb.cpp
void avoidance_adsb_update(void);
#endif
// baro_ground_effect.cpp
void update_ground_effect_detector(void);
@ -965,7 +971,9 @@ private: @@ -965,7 +971,9 @@ private:
ModeStabilize mode_stabilize;
#endif
ModeSport mode_sport;
#if ADSB_ENABLED == ENABLED
ModeAvoidADSB mode_avoid_adsb;
#endif
ModeThrow mode_throw;
ModeGuidedNoGPS mode_guided_nogps;
ModeSmartRTL mode_smartrtl;

4
ArduCopter/GCS_Mavlink.cpp

@ -453,8 +453,10 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id) @@ -453,8 +453,10 @@ bool GCS_MAVLINK_Copter::try_send_message(enum ap_message id)
break;
case MSG_ADSB_VEHICLE:
#if ADSB_ENABLED == ENABLED
CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
copter.adsb.send_adsb_vehicle(chan);
#endif
break;
case MSG_BATTERY_STATUS:
send_battery_status(copter.battery);
@ -683,10 +685,12 @@ void GCS_MAVLINK_Copter::handle_change_alt_request(AP_Mission::Mission_Command & @@ -683,10 +685,12 @@ void GCS_MAVLINK_Copter::handle_change_alt_request(AP_Mission::Mission_Command &
void GCS_MAVLINK_Copter::packetReceived(const mavlink_status_t &status,
mavlink_message_t &msg)
{
#if ADSB_ENABLED == ENABLED
if (copter.g2.dev_options.get() & DevOptionADSBMAVLink) {
// optional handling of GLOBAL_POSITION_INT as a MAVLink based avoidance source
copter.avoidance_adsb.handle_msg(msg);
}
#endif
GCS_MAVLINK::packetReceived(status, msg);
}

2
ArduCopter/Parameters.cpp

@ -761,6 +761,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -761,6 +761,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Path: ../libraries/AP_RPM/AP_RPM.cpp
GOBJECT(rpm_sensor, "RPM", AP_RPM),
#if ADSB_ENABLED == ENABLED
// @Group: ADSB_
// @Path: ../libraries/AP_ADSB/AP_ADSB.cpp
GOBJECT(adsb, "ADSB_", AP_ADSB),
@ -768,6 +769,7 @@ const AP_Param::Info Copter::var_info[] = { @@ -768,6 +769,7 @@ const AP_Param::Info Copter::var_info[] = {
// @Group: AVD_
// @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp
GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Copter),
#endif
// @Param: AUTOTUNE_AXES
// @DisplayName: Autotune axis bitmask

2
ArduCopter/avoidance_adsb.cpp

@ -1,6 +1,7 @@ @@ -1,6 +1,7 @@
#include "Copter.h"
#include <AP_Notify/AP_Notify.h>
#if ADSB_ENABLED == ENABLED
void Copter::avoidance_adsb_update(void)
{
adsb.update();
@ -247,3 +248,4 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs @@ -247,3 +248,4 @@ bool AP_Avoidance_Copter::handle_avoidance_perpendicular(const AP_Avoidance::Obs
// if we got this far we failed to set the new target
return false;
}
#endif

4
ArduCopter/mode.cpp

@ -104,9 +104,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode) @@ -104,9 +104,11 @@ Copter::Mode *Copter::mode_from_mode_num(const uint8_t mode)
ret = &mode_throw;
break;
#if ADSB_ENABLED == ENABLED
case AVOID_ADSB:
ret = &mode_avoid_adsb;
break;
#endif
case GUIDED_NOGPS:
ret = &mode_guided_nogps;
@ -177,7 +179,9 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason) @@ -177,7 +179,9 @@ bool Copter::set_mode(control_mode_t mode, mode_reason_t reason)
control_mode_reason = reason;
DataFlash.Log_Write_Mode(control_mode);
#if ADSB_ENABLED == ENABLED
adsb.set_is_auto_mode((mode == AUTO) || (mode == RTL) || (mode == GUIDED));
#endif
#if AC_FENCE == ENABLED
// pilot requested flight mode change during a fence breach indicates pilot is attempting to manually recover

2
ArduCopter/switches.cpp

@ -554,6 +554,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -554,6 +554,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
break;
case AUXSW_AVOID_ADSB:
#if ADSB_ENABLED == ENABLED
// enable or disable AP_Avoidance
if (ch_flag == AUX_SWITCH_HIGH) {
avoidance_adsb.enable();
@ -562,6 +563,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag) @@ -562,6 +563,7 @@ void Copter::do_aux_switch_function(int8_t ch_function, uint8_t ch_flag)
avoidance_adsb.disable();
Log_Write_Event(DATA_AVOIDANCE_ADSB_DISABLE);
}
#endif
break;
case AUXSW_PRECISION_LOITER:

Loading…
Cancel
Save