Rover: Added Beacon implementation

Enabled beacon parameters
beacon updates at 50hz
Use dataflash library to log AP_Beacon msg

APMrover2/APMrover2.cpp

@@ -51,6 +51,7 @@ const AP_Scheduler::Task Rover::scheduler_tasks[] = {
     SCHED_TASK(update_GPS_50Hz,        50,   2500),
     SCHED_TASK(update_GPS_10Hz,        10,   2500),
     SCHED_TASK(update_alt,             10,   3400),
+    SCHED_TASK(update_beacon,          50,     50),
     SCHED_TASK(navigate,               10,   1600),
     SCHED_TASK(update_compass,         10,   2000),
     SCHED_TASK(update_commands,        10,   1000),
@@ -268,6 +269,7 @@ void Rover::update_logging1(void)
 
     if (should_log(MASK_LOG_CTUN)) {
         Log_Write_Control_Tuning();
+        Log_Write_Beacon();
     }
 
     if (should_log(MASK_LOG_NTUN)) {

APMrover2/Log.cpp

@@ -203,6 +203,16 @@ void Rover::Log_Write_Steering()
     DataFlash.WriteBlock(&pkt, sizeof(pkt));
 }
 
+// Write beacon position and distances
+void Rover::Log_Write_Beacon()
+{
+    // exit immediately if feature is disabled
+    if (!g2.beacon.enabled()) {
+        return;
+    }
+
+    DataFlash.Log_Write_Beacon(g2.beacon);
+}
 struct PACKED log_Startup {
     LOG_PACKET_HEADER;
     uint64_t time_us;

APMrover2/Parameters.cpp

@@ -543,13 +543,18 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     AP_SUBGROUPINFO(afs, "AFS_", 5, ParametersG2, AP_AdvancedFailsafe),
 #endif
 
+    // @Group: BCN
+    // @Path: ../libraries/AP_Beacon/AP_Beacon.cpp
+    AP_SUBGROUPINFO(beacon, "BCN", 6, ParametersG2, AP_Beacon),
+
     AP_GROUPEND
 };
 
 
 ParametersG2::ParametersG2(void)
+    : beacon(rover.serial_manager)
 #if ADVANCED_FAILSAFE == ENABLED
-    : afs(rover.mission, rover.barometer, rover.gps, rover.rcmap)
+    , afs(rover.mission, rover.barometer, rover.gps, rover.rcmap)
 #endif
 {
     AP_Param::setup_object_defaults(this, var_info);

APMrover2/Parameters.h

@@ -318,6 +318,7 @@ public:
     // advanced failsafe library
     AP_AdvancedFailsafe_Rover afs;
 #endif
+    AP_Beacon beacon;
 };
 
 extern const AP_Param::Info var_info[];

APMrover2/Rover.h

@@ -447,6 +447,7 @@ private:
     void Log_Write_Control_Tuning();
     void Log_Write_Nav_Tuning();
     void Log_Write_Sonar();
+    void Log_Write_Beacon();
     void Log_Write_Current();
     void Log_Write_Attitude();
     void Log_Write_RC(void);
@@ -501,6 +502,8 @@ private:
     void trim_radio();
     void init_barometer(bool full_calibration);
     void init_sonar(void);
+    void init_beacon();
+    void update_beacon();
     void read_battery(void);
     void read_receiver_rssi(void);
     void read_sonars(void);

APMrover2/sensors.cpp

@@ -16,6 +16,18 @@ void Rover::init_sonar(void)
     sonar.init();
 }
 
+// init beacons used for non-gps position estimates
+void Rover::init_beacon()
+{
+    g2.beacon.init();
+}
+
+// update beacons
+void Rover::update_beacon()
+{
+    g2.beacon.update();
+}
+
 // read_battery - reads battery voltage and current and invokes failsafe
 // should be called at 10hz
 void Rover::read_battery(void)

APMrover2/system.cpp

@@ -158,6 +158,9 @@ void Rover::init_ardupilot()
     // initialise sonar
     init_sonar();
 
+    // init beacons used for non-gps position estimation
+    init_beacon();
+
     // and baro for EKF
     init_barometer(true);
 
@@ -182,6 +185,9 @@ void Rover::init_ardupilot()
      */
     hal.scheduler->register_timer_failsafe(failsafe_check_static, 1000);
 
+    // give AHRS the range beacon sensor
+    ahrs.set_beacon(&g2.beacon);
+
 
 #if CLI_ENABLED == ENABLED
     // If the switch is in 'menu' mode, run the main menu.