Browse Source

Plane: conditionally compile ADSB support

zr-v5.1
Andy Piper 4 years ago committed by Andrew Tridgell
parent
commit
c057be8ed1
  1. 6
      ArduPlane/ArduPlane.cpp
  2. 6
      ArduPlane/GCS_Mavlink.cpp
  3. 2
      ArduPlane/Parameters.cpp
  4. 5
      ArduPlane/Plane.h
  5. 3
      ArduPlane/avoidance_adsb.cpp
  6. 2
      ArduPlane/is_flying.cpp
  7. 3
      ArduPlane/mode.cpp

6
ArduPlane/ArduPlane.cpp

@ -92,7 +92,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = { @@ -92,7 +92,9 @@ const AP_Scheduler::Task Plane::scheduler_tasks[] = {
SCHED_TASK_CLASS(AP_Logger, &plane.logger, periodic_tasks, 50, 400),
#endif
SCHED_TASK_CLASS(AP_InertialSensor, &plane.ins, periodic, 50, 50),
#if HAL_ADSB_ENABLED
SCHED_TASK(avoidance_adsb_update, 10, 100),
#endif
SCHED_TASK_CLASS(RC_Channels, (RC_Channels*)&plane.g2.rc_channels, read_aux_all, 10, 200),
SCHED_TASK_CLASS(AP_Button, &plane.button, update, 5, 100),
#if STATS_ENABLED == ENABLED
@ -265,10 +267,10 @@ void Plane::one_second_loop() @@ -265,10 +267,10 @@ void Plane::one_second_loop()
// make it possible to change orientation at runtime
ahrs.update_orientation();
#if HAL_ADSB_ENABLED
adsb.set_stall_speed_cm(aparm.airspeed_min);
adsb.set_max_speed(aparm.airspeed_max);
#endif
ahrs.writeDefaultAirSpeed((float)((aparm.airspeed_min + aparm.airspeed_max)/2));
// sync MAVLink system ID

6
ArduPlane/GCS_Mavlink.cpp

@ -434,8 +434,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id) @@ -434,8 +434,10 @@ bool GCS_MAVLINK_Plane::try_send_message(enum ap_message id)
break;
case MSG_ADSB_VEHICLE:
#if HAL_ADSB_ENABLED
CHECK_PAYLOAD_SIZE(ADSB_VEHICLE);
plane.adsb.send_adsb_vehicle(chan);
#endif
break;
case MSG_AOA_SSA:
@ -708,7 +710,9 @@ MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlin @@ -708,7 +710,9 @@ MAV_RESULT GCS_MAVLINK_Plane::_handle_command_preflight_calibration(const mavlin
void GCS_MAVLINK_Plane::packetReceived(const mavlink_status_t &status,
const mavlink_message_t &msg)
{
#if HAL_ADSB_ENABLED
plane.avoidance_adsb.handle_msg(msg);
#endif
GCS_MAVLINK::packetReceived(status, msg);
}
@ -1475,7 +1479,9 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg) @@ -1475,7 +1479,9 @@ void GCS_MAVLINK_Plane::handleMessage(const mavlink_message_t &msg)
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_CFG:
case MAVLINK_MSG_ID_UAVIONIX_ADSB_OUT_DYNAMIC:
case MAVLINK_MSG_ID_UAVIONIX_ADSB_TRANSCEIVER_HEALTH_REPORT:
#if HAL_ADSB_ENABLED
plane.adsb.handle_message(chan, msg);
#endif
break;
default:

2
ArduPlane/Parameters.cpp

@ -911,6 +911,7 @@ const AP_Param::Info Plane::var_info[] = { @@ -911,6 +911,7 @@ const AP_Param::Info Plane::var_info[] = {
GOBJECT(terrain, "TERRAIN_", AP_Terrain),
#endif
#if HAL_ADSB_ENABLED
// @Group: ADSB_
// @Path: ../libraries/AP_ADSB/AP_ADSB.cpp
GOBJECT(adsb, "ADSB_", AP_ADSB),
@ -918,6 +919,7 @@ const AP_Param::Info Plane::var_info[] = { @@ -918,6 +919,7 @@ const AP_Param::Info Plane::var_info[] = {
// @Group: AVD_
// @Path: ../libraries/AP_Avoidance/AP_Avoidance.cpp
GOBJECT(avoidance_adsb, "AVD_", AP_Avoidance_Plane),
#endif
// @Group: Q_
// @Path: quadplane.cpp

5
ArduPlane/Plane.h

@ -108,7 +108,9 @@ @@ -108,7 +108,9 @@
#include "RC_Channel.h" // RC Channel Library
#include "Parameters.h"
#if HAL_ADSB_ENABLED
#include "avoidance_adsb.h"
#endif
#include "AP_Arming.h"
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@ -632,11 +634,12 @@ private: @@ -632,11 +634,12 @@ private:
FUNCTOR_BIND_MEMBER(&Plane::adjusted_relative_altitude_cm, int32_t),
FUNCTOR_BIND_MEMBER(&Plane::disarm_if_autoland_complete, void),
FUNCTOR_BIND_MEMBER(&Plane::update_flight_stage, void)};
#if HAL_ADSB_ENABLED
AP_ADSB adsb;
// avoidance of adsb enabled vehicles (normally manned vheicles)
AP_Avoidance_Plane avoidance_adsb{adsb};
#endif
// Outback Challenge Failsafe Support
#if ADVANCED_FAILSAFE == ENABLED

3
ArduPlane/avoidance_adsb.cpp

@ -2,6 +2,7 @@ @@ -2,6 +2,7 @@
#include <stdio.h>
#include "Plane.h"
#if HAL_ADSB_ENABLED
void Plane::avoidance_adsb_update(void)
{
adsb.update();
@ -202,3 +203,5 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl @@ -202,3 +203,5 @@ bool AP_Avoidance_Plane::handle_avoidance_horizontal(const AP_Avoidance::Obstacl
return false;
}
#endif

2
ArduPlane/is_flying.cpp

@ -154,7 +154,9 @@ void Plane::update_is_flying_5Hz(void) @@ -154,7 +154,9 @@ void Plane::update_is_flying_5Hz(void)
}
}
previous_is_flying = new_is_flying;
#if HAL_ADSB_ENABLED
adsb.set_is_flying(new_is_flying);
#endif
#if PARACHUTE == ENABLED
parachute.set_is_flying(new_is_flying);
#endif

3
ArduPlane/mode.cpp

@ -72,8 +72,9 @@ bool Mode::enter() @@ -72,8 +72,9 @@ bool Mode::enter()
// start with throttle suppressed in auto_throttle modes
plane.throttle_suppressed = plane.auto_throttle_mode;
#if HAL_ADSB_ENABLED
plane.adsb.set_is_auto_mode(plane.auto_navigation_mode);
#endif
// reset steering integrator on mode change
plane.steerController.reset_I();

Loading…
Cancel
Save