AP_Periph: add MAVLink support for AP_Periph modules

Tools/AP_Periph/AP_Periph.h

@@ -23,6 +23,10 @@
 #include <AP_HAL_SITL/CANSocketIface.h>
 #endif
 
+#ifndef HAL_NO_GCS
+#include "GCS_MAVLink.h"
+#endif
+
 #if defined(HAL_PERIPH_NEOPIXEL_COUNT_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_NCP5623_BGR_LED_WITHOUT_NOTIFY) || defined(HAL_PERIPH_ENABLE_TOSHIBA_LED_WITHOUT_NOTIFY)
 #define AP_PERIPH_HAVE_LED_WITHOUT_NOTIFY
 #endif
@@ -210,6 +214,9 @@ public:
     AP_Logger logger;
 #endif
 
+#ifndef HAL_NO_GCS
+    GCS_Periph _gcs;
+#endif
     // setup the var_info table
     AP_Param param_loader{var_info};
 

Tools/AP_Periph/GCS_MAVLink.cpp

@@ -0,0 +1,57 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "GCS_MAVLink.h"
+#include "AP_Periph.h"
+
+#ifndef HAL_NO_GCS
+
+static const ap_message STREAM_RAW_SENSORS_msgs[] = {
+    MSG_RAW_IMU
+};
+static const ap_message STREAM_EXTENDED_STATUS_msgs[] = {
+    MSG_SYS_STATUS,
+    MSG_POWER_STATUS,
+    MSG_MEMINFO,
+    MSG_GPS_RAW,
+    MSG_GPS_RTK,
+};
+
+static const ap_message STREAM_POSITION_msgs[] = {
+    MSG_LOCATION,
+    MSG_LOCAL_POSITION
+};
+
+static const ap_message STREAM_PARAMS_msgs[] = {
+    MSG_NEXT_PARAM
+};
+
+const struct GCS_MAVLINK::stream_entries GCS_MAVLINK::all_stream_entries[] = {
+    MAV_STREAM_ENTRY(STREAM_RAW_SENSORS),
+    MAV_STREAM_ENTRY(STREAM_POSITION),
+    MAV_STREAM_ENTRY(STREAM_EXTENDED_STATUS),
+    MAV_STREAM_ENTRY(STREAM_PARAMS),
+    MAV_STREAM_TERMINATOR // must have this at end of stream_entries
+};
+
+const struct AP_Param::GroupInfo GCS_MAVLINK_Parameters::var_info[] = {
+    AP_GROUPEND
+};
+
+uint8_t GCS_MAVLINK_Periph::sysid_my_gcs() const
+{
+    return periph.g.sysid_this_mav;
+}
+#endif // #ifndef HAL_NO_GCS

Tools/AP_Periph/GCS_MAVLink.h

@@ -0,0 +1,91 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+#pragma once
+
+#include <GCS_MAVLink/GCS.h>
+#ifndef HAL_NO_GCS
+
+/*
+ *  GCS backend used for many examples and tools
+ */
+class GCS_MAVLINK_Periph : public GCS_MAVLINK
+{
+public:
+
+    using GCS_MAVLINK::GCS_MAVLINK;
+
+private:
+
+    uint32_t telem_delay() const override { return 0; }
+    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 {}
+
+protected:
+
+    uint8_t sysid_my_gcs() const override;
+
+    // Periph information:
+    MAV_MODE base_mode() const override { return (MAV_MODE)MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; }
+    MAV_STATE vehicle_system_status() const override { return MAV_STATE_CALIBRATING; }
+
+    bool set_home_to_current_location(bool _lock) override { return false; }
+    bool set_home(const Location& loc, bool _lock) override { return false; }
+
+    void send_nav_controller_output() const override {};
+    void send_pid_tuning() override {};
+};
+
+/*
+ * a GCS singleton used for many example sketches and tools
+ */
+
+extern const AP_HAL::HAL& hal;
+
+class GCS_Periph : public GCS
+{
+public:
+
+    using GCS::GCS;
+
+protected:
+
+    uint8_t sysid_this_mav() const override { return 1; }
+
+    GCS_MAVLINK_Periph *new_gcs_mavlink_backend(GCS_MAVLINK_Parameters &params,
+                                               AP_HAL::UARTDriver &uart) override {
+        return new GCS_MAVLINK_Periph(params, uart);
+    }
+
+private:
+    GCS_MAVLINK_Periph *chan(const uint8_t ofs) override {
+        if (ofs > _num_gcs) {
+            INTERNAL_ERROR(AP_InternalError::error_t::gcs_offset);
+            return nullptr;
+        }
+        return (GCS_MAVLINK_Periph *)_chan[ofs];
+    };
+    const GCS_MAVLINK_Periph *chan(const uint8_t ofs) const override {
+        if (ofs > _num_gcs) {
+            INTERNAL_ERROR(AP_InternalError::error_t::gcs_offset);
+            return nullptr;
+        }
+        return (GCS_MAVLINK_Periph *)_chan[ofs];
+    };
+
+    MAV_TYPE frame_type() const override { return MAV_TYPE_GENERIC; }
+    uint32_t custom_mode() const override { return 3; } // magic number
+};
+#endif // HAL_NO_GCS

Tools/AP_Periph/Parameters.cpp

@@ -29,6 +29,12 @@ extern const AP_HAL::HAL &hal;
 #define AP_PERIPH_MSP_PORT_DEFAULT 1
 #endif
 
+#ifndef HAL_DEFAULT_MAV_SYSTEM_ID
+#define MAV_SYSTEM_ID 3
+#else
+#define MAV_SYSTEM_ID HAL_DEFAULT_MAV_SYSTEM_ID
+#endif
+
 /*
  *  AP_Periph parameter definitions
  *
@@ -318,6 +324,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GSCALAR(log_bitmask,    "LOG_BITMASK",          4),
 #endif
 
+#ifndef HAL_NO_GCS
+    // @Param: SYSID_THISMAV
+    // @DisplayName: MAVLink system ID of this vehicle
+    // @Description: Allows setting an individual system id for this vehicle to distinguish it from others on the same network
+    // @Range: 1 255
+    // @User: Advanced
+    GSCALAR(sysid_this_mav,         "SYSID_THISMAV",  MAV_SYSTEM_ID),
+#endif
+
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_Common/AP_Common.h>
+#include "GCS_MAVLink.h"
 
 // Global parameter class.
 //
@@ -45,6 +46,7 @@ public:
         k_param_can_protocol0,
         k_param_can_protocol1,
         k_param_can_protocol2,
+        k_param_sysid_this_mav,
     };
 
     AP_Int16 format_version;
@@ -107,6 +109,10 @@ public:
     AP_Int32        log_bitmask;
 #endif
 
+#ifndef HAL_NO_GCS
+    AP_Int16 sysid_this_mav;
+#endif
+
     Parameters() {}
 };
 

Tools/AP_Periph/wscript

@@ -12,41 +12,45 @@ def build(bld):
     if not valid_target:
         return
 
+    if bld.env.AP_PERIPH_HEAVY:
+        libraries = bld.ap_get_all_libraries()
+        bld.env.DEFINES += ['HAL_BOARD_AP_PERIPH_HEAVY']
+    else:
+        libraries = ['AP_Common',
+                     'AP_HAL',
+                     'AP_HAL_Empty',
+                     'AP_Math',
+                     'AP_BoardConfig',
+                     'AP_BattMonitor',
+                     'AP_CANManager',
+                     'AP_Param',
+                     'StorageManager',
+                     'AP_FlashStorage',
+                     'AP_RAMTRON',
+                     'AP_GPS',
+                     'AP_SerialManager',
+                     'AP_RTC',
+                     'AP_Compass',
+                     'AP_Baro',
+                     'Filter',
+                     'AP_InternalError',
+                     'AP_Airspeed',
+                     'AP_RangeFinder',
+                     'AP_ROMFS',
+                     'AP_MSP',
+                     'SRV_Channel',
+                     'AP_Notify',
+                     'AP_SerialLED',
+                     'AP_Filesystem',
+                     'AP_InertialSensor',
+                     'AP_AccelCal',
+                     'AP_Logger',
+                     'AC_PID',
+                     ]
     bld.ap_stlib(
         name= 'AP_Periph_libs',
         ap_vehicle='AP_Periph',
-        ap_libraries= [
-        'AP_Common',
-        'AP_HAL',
-        'AP_HAL_Empty',
-        'AP_Math',
-        'AP_BoardConfig',
-        'AP_BattMonitor',
-        'AP_CANManager',
-        'AP_Param',
-        'StorageManager',
-        'AP_FlashStorage',
-        'AP_RAMTRON',
-        'AP_GPS',
-        'AP_SerialManager',
-        'AP_RTC',
-        'AP_Compass',
-        'AP_Baro',
-        'Filter',
-        'AP_InternalError',
-        'AP_Airspeed',
-        'AP_RangeFinder',
-        'AP_ROMFS',
-        'AP_MSP',
-        'SRV_Channel',
-        'AP_Notify',
-        'AP_SerialLED',
-        'AP_Filesystem',
-        'AP_InertialSensor',
-        'AP_AccelCal',
-        'AP_Logger',
-        'AC_PID',
-        ],
+        ap_libraries= libraries,
         exclude_src=[
             'libraries/AP_HAL_ChibiOS/Storage.cpp'
         ]