From 47ff9ddeecfc0fcc9848c697b4264b5c632510ec Mon Sep 17 00:00:00 2001
From: "karthik.desai" <karthik.desai@iav.de>
Date: Mon, 3 Apr 2017 22:46:12 +0200
Subject: [PATCH] Rover: Added Beacon implementation

Enabled beacon parameters
beacon updates at 50hz
Use dataflash library to log AP_Beacon msg
---
 APMrover2/APMrover2.cpp  |  2 ++
 APMrover2/Log.cpp        | 10 ++++++++++
 APMrover2/Parameters.cpp |  7 ++++++-
 APMrover2/Parameters.h   |  1 +
 APMrover2/Rover.h        |  3 +++
 APMrover2/sensors.cpp    | 12 ++++++++++++
 APMrover2/system.cpp     |  6 ++++++
 7 files changed, 40 insertions(+), 1 deletion(-)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 84cc3d4de6..29f0919930 100644
--- a/APMrover2/APMrover2.cpp
+++ b/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)) {
diff --git a/APMrover2/Log.cpp b/APMrover2/Log.cpp
index 5f8f896f5f..6d5439b07f 100644
--- a/APMrover2/Log.cpp
+++ b/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;
diff --git a/APMrover2/Parameters.cpp b/APMrover2/Parameters.cpp
index 56a02533aa..c0c693dbac 100644
--- a/APMrover2/Parameters.cpp
+++ b/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);
diff --git a/APMrover2/Parameters.h b/APMrover2/Parameters.h
index 5fefd4f544..fe010ab092 100644
--- a/APMrover2/Parameters.h
+++ b/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[];
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index d29adf2669..8590369333 100644
--- a/APMrover2/Rover.h
+++ b/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);
diff --git a/APMrover2/sensors.cpp b/APMrover2/sensors.cpp
index 50cb506a26..f2cf3c4404 100644
--- a/APMrover2/sensors.cpp
+++ b/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)
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index fccab22511..acb73a3698 100644
--- a/APMrover2/system.cpp
+++ b/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.