From 637ccf13df9fbf08832bdb1601409cc7d90e22a5 Mon Sep 17 00:00:00 2001 From: Siddharth Purohit Date: Sun, 20 Jun 2021 11:32:12 +0530 Subject: [PATCH] AP_Periph: add MAVLink support for AP_Periph modules --- Tools/AP_Periph/AP_Periph.h | 7 +++ Tools/AP_Periph/GCS_MAVLink.cpp | 57 +++++++++++++++++++++ Tools/AP_Periph/GCS_MAVLink.h | 91 +++++++++++++++++++++++++++++++++ Tools/AP_Periph/Parameters.cpp | 15 ++++++ Tools/AP_Periph/Parameters.h | 6 +++ Tools/AP_Periph/wscript | 68 ++++++++++++------------ 6 files changed, 212 insertions(+), 32 deletions(-) create mode 100644 Tools/AP_Periph/GCS_MAVLink.cpp create mode 100644 Tools/AP_Periph/GCS_MAVLink.h diff --git a/Tools/AP_Periph/AP_Periph.h b/Tools/AP_Periph/AP_Periph.h index d8f8dd14e2..d015a6c3aa 100644 --- a/Tools/AP_Periph/AP_Periph.h +++ b/Tools/AP_Periph/AP_Periph.h @@ -23,6 +23,10 @@ #include #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}; diff --git a/Tools/AP_Periph/GCS_MAVLink.cpp b/Tools/AP_Periph/GCS_MAVLink.cpp new file mode 100644 index 0000000000..5338b7067a --- /dev/null +++ b/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 . + */ + +#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 diff --git a/Tools/AP_Periph/GCS_MAVLink.h b/Tools/AP_Periph/GCS_MAVLink.h new file mode 100644 index 0000000000..0944d6b735 --- /dev/null +++ b/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 . + */ +#pragma once + +#include +#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 ¶ms, + 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 diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index c8bdc00f2b..b0c9432f07 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/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 }; diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h index 0f54b76b1c..a2187409e2 100644 --- a/Tools/AP_Periph/Parameters.h +++ b/Tools/AP_Periph/Parameters.h @@ -1,6 +1,7 @@ #pragma once #include +#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() {} }; diff --git a/Tools/AP_Periph/wscript b/Tools/AP_Periph/wscript index 084c142984..5bef001980 100644 --- a/Tools/AP_Periph/wscript +++ b/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' ]