From 35d94b17eba2423d6be7b7966fa30ed821158cda Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Sun, 11 Jul 2021 12:08:42 +0530
Subject: [PATCH] AP_Periph: add support for MAVLink for HerePro

---
 Tools/AP_Periph/AP_Periph.cpp   | 30 +++++++++++++++++++++++++-----
 Tools/AP_Periph/GCS_MAVLink.cpp | 20 ++++++++++++++++++++
 Tools/AP_Periph/GCS_MAVLink.h   |  3 ++-
 Tools/AP_Periph/Parameters.cpp  |  4 ++++
 Tools/AP_Periph/Parameters.h    |  1 +
 5 files changed, 52 insertions(+), 6 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index ef2025415a..2738f30792 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -92,7 +92,9 @@ void AP_Periph_FW::init()
 
     stm32_watchdog_pat();
 
+#ifdef HAL_NO_GCS
     hal.serial(0)->begin(AP_SERIALMANAGER_CONSOLE_BAUD, 32, 32);
+#endif
     hal.serial(3)->begin(115200, 128, 256);
 
     load_parameters();
@@ -101,8 +103,17 @@ void AP_Periph_FW::init()
 
     can_start();
 
+#ifndef HAL_NO_GCS
+    stm32_watchdog_pat();
+    gcs().init();
+#endif
     serial_manager.init();
 
+#ifndef HAL_NO_GCS
+    gcs().setup_console();
+    gcs().send_text(MAV_SEVERITY_INFO, "AP_Periph GCS Initialised!");
+#endif
+
     stm32_watchdog_pat();
 
 #ifdef HAL_BOARD_AP_PERIPH_ZUBAXGNSS
@@ -337,6 +348,11 @@ void AP_Periph_FW::update()
 #ifdef HAL_PERIPH_ENABLE_RC_OUT
         rcout_init_1Hz();
 #endif
+
+#ifndef HAL_NO_GCS
+        gcs().send_message(MSG_HEARTBEAT);
+        gcs().send_message(MSG_SYS_STATUS);
+#endif    
     }
 
     static uint32_t last_error_ms;
@@ -363,14 +379,18 @@ void AP_Periph_FW::update()
     }
 #endif
 
+    static uint32_t fiftyhz_last_update_ms;
+    if (now - fiftyhz_last_update_ms >= 20) {
+        // update at 50Hz
+        fiftyhz_last_update_ms = now;
 #ifdef HAL_PERIPH_ENABLE_NOTIFY
-    static uint32_t notify_last_update_ms;
-    if (now - notify_last_update_ms >= 20) {
-        // update notify at 50Hz
-        notify_last_update_ms = now;
         notify.update();
-    }
 #endif
+#ifndef HAL_NO_GCS
+        gcs().update_receive();
+        gcs().update_send();
+#endif
+    }
 
 #if HAL_LOGGING_ENABLED
     logger.periodic_tasks();
diff --git a/Tools/AP_Periph/GCS_MAVLink.cpp b/Tools/AP_Periph/GCS_MAVLink.cpp
index 5338b7067a..ec87322519 100644
--- a/Tools/AP_Periph/GCS_MAVLink.cpp
+++ b/Tools/AP_Periph/GCS_MAVLink.cpp
@@ -30,8 +30,10 @@ static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
 };
 
 static const ap_message STREAM_POSITION_msgs[] = {
+#if defined(HAL_PERIPH_ENABLE_AHRS)
     MSG_LOCATION,
     MSG_LOCAL_POSITION
+#endif
 };
 
 static const ap_message STREAM_PARAMS_msgs[] = {
@@ -54,4 +56,22 @@ uint8_t GCS_MAVLINK_Periph::sysid_my_gcs() const
 {
     return periph.g.sysid_this_mav;
 }
+
+uint8_t GCS_Periph::sysid_this_mav() const
+{
+    return periph.g.sysid_this_mav;
+}
+
+MAV_RESULT GCS_MAVLINK_Periph::handle_preflight_reboot(const mavlink_command_long_t &packet)
+{
+    printf("RestartNode\n");
+    hal.scheduler->delay(10);
+    periph.prepare_reboot();
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+    NVIC_SystemReset();
+#elif CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    HAL_SITL::actually_reboot();
+#endif
+}
+
 #endif // #ifndef HAL_NO_GCS
diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h
index 0944d6b735..8b3305e67c 100644
--- a/Tools/AP_Periph/GCS_MAVLink.h
+++ b/Tools/AP_Periph/GCS_MAVLink.h
@@ -32,6 +32,7 @@ private:
     void handleMessage(const mavlink_message_t &msg) override { handle_common_message(msg); }
     bool handle_guided_request(AP_Mission::Mission_Command &cmd) override { return true; }
     void handle_change_alt_request(AP_Mission::Mission_Command &cmd) override {}
+    MAV_RESULT handle_preflight_reboot(const mavlink_command_long_t &packet) override;
 
 protected:
 
@@ -62,7 +63,7 @@ public:
 
 protected:
 
-    uint8_t sysid_this_mav() const override { return 1; }
+    uint8_t sysid_this_mav() const override;
 
     GCS_MAVLINK_Periph *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
                                                AP_HAL::UARTDriver &uart) override {
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index b0c9432f07..5082caa917 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -331,6 +331,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Range: 1 255
     // @User: Advanced
     GSCALAR(sysid_this_mav,         "SYSID_THISMAV",  MAV_SYSTEM_ID),
+
+    // @Group: SERIAL
+    // @Path: ../libraries/AP_SerialManager/AP_SerialManager.cpp
+    GOBJECT(serial_manager, "SERIAL",   AP_SerialManager),
 #endif
 
     AP_VAREND
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index a2187409e2..478609b02f 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -47,6 +47,7 @@ public:
         k_param_can_protocol1,
         k_param_can_protocol2,
         k_param_sysid_this_mav,
+        k_param_serial_manager,
     };
 
     AP_Int16 format_version;