Copter: add define for disabling beacon

This commit is contained in:
Peter Barker 2018-02-22 15:53:20 +11:00 committed by Randy Mackay
parent 18135b4764
commit b8e1f03599
9 changed files with 25 additions and 22 deletions

View File

@ -21,6 +21,7 @@
//#define FRSKY_TELEM_ENABLED DISABLED // disable FRSky telemetry
//#define ADSB_ENABLED DISABLED // disable ADSB support
//#define PRECISION_LANDING DISABLED // disable precision landing using companion computer or IRLock sensor
//#define BEACON_ENABLED DISABLED // disable beacon support
//#define SPRAYER DISABLED // disable the crop sprayer feature (two ESC controlled pumps the speed of which depends upon the vehicle's horizontal velocity)
//#define WINCH_ENABLED DISABLED // disable winch support
//#define MODE_AUTO_ENABLED DISABLED // disable auto mode support

View File

@ -101,7 +101,9 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = {
#if PROXIMITY_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Proximity, &copter.g2.proximity, update, 100, 50),
#endif
#if BEACON_ENABLED == ENABLED
SCHED_TASK_CLASS(AP_Beacon, &copter.g2.beacon, update, 400, 50),
#endif
SCHED_TASK(update_visual_odom, 400, 50),
SCHED_TASK(update_altitude, 10, 100),
SCHED_TASK(run_nav_updates, 50, 100),
@ -336,7 +338,9 @@ void Copter::ten_hz_logging_loop()
if (should_log(MASK_LOG_CTUN)) {
attitude_control->control_monitor_log();
Log_Write_Proximity();
Log_Write_Beacon();
#if BEACON_ENABLED == ENABLED
DataFlash.Log_Write_Beacon(g2.beacon);
#endif
}
#if FRAME_CONFIG == HELI_FRAME
Log_Write_Heli();

View File

@ -797,7 +797,6 @@ private:
void Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target);
void Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool position_ok);
void Log_Write_Proximity();
void Log_Write_Beacon();
void Log_Write_Vehicle_Startup_Messages();
void log_init(void);
@ -883,7 +882,6 @@ private:
void init_proximity();
void update_proximity();
void update_sensor_status_flags(void);
void init_beacon();
void init_visual_odom();
void update_visual_odom();
void winch_init();

View File

@ -579,16 +579,6 @@ void Copter::Log_Write_Proximity()
#endif
}
// Write beacon position and distances
void Copter::Log_Write_Beacon()
{
// exit immediately if feature is disabled
if (!g2.beacon.enabled()) {
return;
}
DataFlash.Log_Write_Beacon(g2.beacon);
}
// type and unit information can be found in
// libraries/DataFlash/Logstructure.h; search for "log_Units" for
// units and "Format characters" for field type information
@ -675,7 +665,6 @@ void Copter::Log_Write_Precland() {}
void Copter::Log_Write_GuidedTarget(uint8_t target_type, const Vector3f& pos_target, const Vector3f& vel_target) {}
void Copter::Log_Write_Throw(ThrowModeStage stage, float velocity, float velocity_z, float accel, float ef_accel_z, bool throw_detect, bool attitude_ok, bool height_ok, bool pos_ok) {}
void Copter::Log_Write_Proximity() {}
void Copter::Log_Write_Beacon() {}
void Copter::Log_Write_Vehicle_Startup_Messages() {}
#if FRAME_CONFIG == HELI_FRAME

View File

@ -874,7 +874,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
// @Group: BCN
// @Path: ../libraries/AP_Beacon/AP_Beacon.cpp
#if BEACON_ENABLED == ENABLED
AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon),
#endif
#if PROXIMITY_ENABLED == ENABLED
// @Group: PRX
@ -994,7 +996,10 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
constructor for g2 object
*/
ParametersG2::ParametersG2(void)
: beacon(copter.serial_manager)
: temp_calibration(copter.barometer, copter.ins)
#if BEACON_ENABLED == ENABLED
, beacon(copter.serial_manager)
#endif
#if PROXIMITY_ENABLED == ENABLED
, proximity(copter.serial_manager)
#endif

View File

@ -512,8 +512,10 @@ public:
// ground effect compensation enable/disable
AP_Int8 gndeffect_comp_enabled;
#if BEACON_ENABLED == ENABLED
// beacon (non-GPS positioning) library
AP_Beacon beacon;
#endif
#if VISUAL_ODOMETRY_ENABLED == ENABLED
// Visual Odometry camera

View File

@ -333,6 +333,12 @@
# define MODE_SPORT_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// Beacon support - support for local positioning systems
#ifndef BEACON_ENABLED
# define BEACON_ENABLED ENABLED
#endif
//////////////////////////////////////////////////////////////////////////////
// RADIO CONFIGURATION
//////////////////////////////////////////////////////////////////////////////

View File

@ -467,12 +467,6 @@ void Copter::update_sensor_status_flags(void)
#endif
}
// init beacons used for non-gps position estimates
void Copter::init_beacon()
{
g2.beacon.init();
}
// init visual odometry sensor
void Copter::init_visual_odom()
{

View File

@ -153,8 +153,10 @@ void Copter::init_ardupilot()
*/
hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
#if BEACON_ENABLED == ENABLED
// give AHRS the range beacon sensor
ahrs.set_beacon(&g2.beacon);
#endif
// Do GPS init
gps.set_log_gps_bit(MASK_LOG_GPS);
@ -222,8 +224,10 @@ void Copter::init_ardupilot()
// init proximity sensor
init_proximity();
#if BEACON_ENABLED == ENABLED
// init beacons used for non-gps position estimation
init_beacon();
g2.beacon.init();
#endif
// init visual odometry
init_visual_odom();