diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 4cdc3525af..753c03d84f 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -94,6 +94,7 @@ const AP_Scheduler::Task Copter::scheduler_tasks[] = { SCHED_TASK(auto_trim, 10, 75), SCHED_TASK(read_rangefinder, 20, 100), SCHED_TASK(update_proximity, 100, 50), + SCHED_TASK(update_beacon, 100, 50), SCHED_TASK(update_altitude, 10, 100), SCHED_TASK(run_nav_updates, 50, 100), SCHED_TASK(update_throttle_hover,100, 90), diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index 93e7c465a5..c1e24e214e 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -61,6 +61,7 @@ #include // Range finder library #include #include // statistics library +#include #include // Optical Flow library #include // RSSI Library #include // Filter library @@ -716,6 +717,8 @@ private: void init_proximity(); void update_proximity(); void stats_update(); + void init_beacon(); + void update_beacon(); void send_pid_tuning(mavlink_channel_t chan); void gcs_send_message(enum ap_message id); void gcs_send_mission_item_reached_message(uint16_t mission_index); diff --git a/ArduCopter/Parameters.cpp b/ArduCopter/Parameters.cpp index cadd0dd69d..a67def7867 100644 --- a/ArduCopter/Parameters.cpp +++ b/ArduCopter/Parameters.cpp @@ -1039,6 +1039,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { AP_SUBGROUPINFO(gripper, "GRIP_", 13, ParametersG2, AP_Gripper), #endif + // @Group: BCN + // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp + AP_SUBGROUPINFO(beacon, "BCN", 14, ParametersG2, AP_Beacon), AP_GROUPEND }; @@ -1048,7 +1051,8 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = { constructor for g2 object */ ParametersG2::ParametersG2(void) - : proximity(copter.serial_manager) + : proximity(copter.serial_manager), + beacon(copter.serial_manager) #if ADVANCED_FAILSAFE == ENABLED ,afs(copter.mission, copter.barometer, copter.gps, copter.rcmap) #endif diff --git a/ArduCopter/Parameters.h b/ArduCopter/Parameters.h index b82b5c7a15..bf190bc663 100644 --- a/ArduCopter/Parameters.h +++ b/ArduCopter/Parameters.h @@ -569,6 +569,9 @@ public: AP_Proximity proximity; #endif + // beacon (non-GPS positioning) library + AP_Beacon beacon; + // whether to enforce acceptance of packets only from sysid_my_gcs AP_Int8 sysid_enforce; diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index 854f599ee1..3eeb05d168 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -434,3 +434,15 @@ void Copter::update_sensor_status_flags(void) frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present); #endif } + +// init beacons used for non-gps position estimates +void Copter::init_beacon() +{ + g2.beacon.init(); +} + +// update beacons +void Copter::update_beacon() +{ + g2.beacon.update(); +} diff --git a/ArduCopter/system.cpp b/ArduCopter/system.cpp index 2fc4d82300..6b2f3b231b 100644 --- a/ArduCopter/system.cpp +++ b/ArduCopter/system.cpp @@ -262,6 +262,9 @@ void Copter::init_ardupilot() // init proximity sensor init_proximity(); + // init beacons used for non-gps position estimation + init_beacon(); + // initialise AP_RPM library rpm_sensor.init();