diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py index dba7c96157764..6dd1ff38a0acd 100644 --- a/Tools/autotest/rover.py +++ b/Tools/autotest/rover.py @@ -5428,6 +5428,28 @@ def test_scripting_set_home_to_vehicle_location(self): self.context_pop() self.reboot_sitl() + def test_scripting_serial_loopback(self): + self.start_subtest("Scripting serial loopback test") + + self.context_push() + self.context_collect('STATUSTEXT') + self.set_parameters({ + "SCR_ENABLE": 1, + "SCR_SDEV_EN": 1, + "SCR_SDEV1_PROTO": 28, + }) + self.install_test_script_context("serial_loopback.lua") + self.reboot_sitl() + + for success_text in [ + "driver -> device good", + "device -> driver good", + ]: + self.wait_statustext(success_text, check_context=True) + + self.context_pop() + self.reboot_sitl() + def Scripting(self): '''Scripting test''' self.test_scripting_set_home_to_vehicle_location() @@ -5436,6 +5458,7 @@ def Scripting(self): self.test_scripting_simple_loop() self.test_scripting_internal_test() self.test_scripting_auxfunc() + self.test_scripting_serial_loopback() def test_mission_frame(self, frame, target_system=1, target_component=1): self.clear_mission(mavutil.mavlink.MAV_MISSION_TYPE_MISSION, diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py index a71c545846ade..ce45cc9f98bbe 100644 --- a/Tools/scripts/build_options.py +++ b/Tools/scripts/build_options.py @@ -315,7 +315,8 @@ def __init__(self, Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA Output', 0, None), Feature('Other', 'SDCARD_FORMATTING', 'AP_FILESYSTEM_FORMAT_ENABLED', 'Enable formatting of microSD cards', 0, None), Feature('Other', 'BOOTLOADER_FLASHING', 'AP_BOOTLOADER_FLASHING_ENABLED', 'Enable Bootloader flashing', 0, "FILESYSTEM_ROMFS"), # noqa - Feature('Other', 'SCRIPTING', 'AP_SCRIPTING_ENABLED', 'Enable LUA Scripting', 0, None), + Feature('Other', 'SCRIPTING', 'AP_SCRIPTING_ENABLED', 'Enable Lua scripting', 0, None), + Feature('Other', 'SCRIPTING_SERIALDEVICE', 'AP_SCRIPTING_SERIALDEVICE_ENABLED', 'Enable Lua serial device simulation', 0, "SCRIPTING"), # noqa Feature('Other', 'SLCAN', 'AP_CAN_SLCAN_ENABLED', 'Enable SLCAN serial protocol', 0, None), Feature('Other', 'SDCARD_MISSION', 'AP_SDCARD_STORAGE_ENABLED', 'Enable storing mission on microSD cards', 0, None), Feature('Other', 'COMPASS_CAL', 'COMPASS_CAL_ENABLED', 'Enable "tumble" compass calibration', 0, None), diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py index 659b2a1a9468b..ed5eaf3f71144 100755 --- a/Tools/scripts/extract_features.py +++ b/Tools/scripts/extract_features.py @@ -207,6 +207,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"): ('AP_RC_CHANNEL_AUX_FUNCTION_STRINGS_ENABLED', r'RC_Channel::lookuptable',), ('AP_SCRIPTING_ENABLED', r'AP_Scripting::init',), + ('AP_SCRIPTING_SERIALDEVICE_ENABLED', r'AP_Scripting_SerialDevice::init',), ('AP_NOTIFY_TONEALARM_ENABLED', r'AP_ToneAlarm::init'), ('AP_NOTIFY_MAVLINK_PLAY_TUNE_SUPPORT_ENABLED', r'AP_Notify::handle_play_tune'), diff --git a/libraries/AP_Scripting/AP_Scripting.cpp b/libraries/AP_Scripting/AP_Scripting.cpp index b283f9791a947..b4056d7139b04 100644 --- a/libraries/AP_Scripting/AP_Scripting.cpp +++ b/libraries/AP_Scripting/AP_Scripting.cpp @@ -160,6 +160,43 @@ const AP_Param::GroupInfo AP_Scripting::var_info[] = { // @RebootRequired: True // @User: Advanced AP_GROUPINFO("THD_PRIORITY", 14, AP_Scripting, _thd_priority, uint8_t(ThreadPriority::NORMAL)), + +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + // @Param: SDEV_EN + // @DisplayName: Scripting serial device enable + // @Description: Enable scripting serial devices + // @Values: 0:Disabled, 1:Enabled + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO_FLAGS("SDEV_EN", 15, AP_Scripting, _serialdevice.enable, 0, AP_PARAM_FLAG_ENABLE), + + // @Param: SDEV1_PROTO + // @DisplayName: Serial protocol of scripting serial device + // @Description: Serial protocol of scripting serial device + // @CopyFieldsFrom: SERIAL1_PROTOCOL + // @RebootRequired: True + // @User: Advanced + AP_GROUPINFO("SDEV1_PROTO", 16, AP_Scripting, _serialdevice.ports[0].state.protocol, -1), + +#if AP_SCRIPTING_SERIALDEVICE_NUM_PORTS > 1 + // @Param: SDEV2_PROTO + // @DisplayName: Serial protocol of scripting serial device + // @Description: Serial protocol of scripting serial device + // @CopyFieldsFrom: SCR_SDEV1_PROTO + AP_GROUPINFO("SDEV2_PROTO", 17, AP_Scripting, _serialdevice.ports[1].state.protocol, -1), +#endif + +#if AP_SCRIPTING_SERIALDEVICE_NUM_PORTS > 2 + // @Param: SDEV3_PROTO + // @DisplayName: Serial protocol of scripting serial device + // @Description: Serial protocol of scripting serial device + // @CopyFieldsFrom: SCR_SDEV1_PROTO + AP_GROUPINFO("SDEV3_PROTO", 18, AP_Scripting, _serialdevice.ports[2].state.protocol, -1), +#endif +#endif // AP_SCRIPTING_SERIALDEVICE_ENABLED + + // WARNING: additional parameters must be listed before SDEV_EN (but have an + // index after SDEV3_PROTO) so they are not disabled by it! AP_GROUPEND }; @@ -220,6 +257,16 @@ void AP_Scripting::init(void) { } } +#if AP_SCRIPTING_SERIALDEVICE_ENABLED +void AP_Scripting::init_serialdevice_ports(void) { + if (!_enable) { + return; + } + + _serialdevice.init(); +} +#endif + #if HAL_GCS_ENABLED MAV_RESULT AP_Scripting::handle_command_int_packet(const mavlink_command_int_t &packet) { switch ((SCRIPTING_CMD)packet.param1) { @@ -264,6 +311,11 @@ void AP_Scripting::thread(void) { GCS_SEND_TEXT(MAV_SEVERITY_CRITICAL, "Scripting: %s", "Unable to allocate memory"); _init_failed = true; } else { +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + // clear data in serial buffers that the script wasn't ready to + // receive + _serialdevice.clear(); +#endif // run won't return while scripting is still active lua->run(); @@ -298,6 +350,11 @@ void AP_Scripting::thread(void) { } } #endif // AP_NETWORKING_ENABLED + +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + // clear data in serial buffers that hasn't been transmitted + _serialdevice.clear(); +#endif // Clear blocked commands { diff --git a/libraries/AP_Scripting/AP_Scripting.h b/libraries/AP_Scripting/AP_Scripting.h index a2b22e45de4e4..234917bbd55f7 100644 --- a/libraries/AP_Scripting/AP_Scripting.h +++ b/libraries/AP_Scripting/AP_Scripting.h @@ -39,6 +39,14 @@ class SocketAPM; #endif +#ifndef AP_SCRIPTING_SERIALDEVICE_ENABLED +#define AP_SCRIPTING_SERIALDEVICE_ENABLED AP_SERIALMANAGER_REGISTER_ENABLED && (BOARD_FLASH_SIZE>1024) +#endif + +#if AP_SCRIPTING_SERIALDEVICE_ENABLED +#include "AP_Scripting_SerialDevice.h" +#endif + class AP_Scripting { public: @@ -49,6 +57,10 @@ class AP_Scripting void init(void); +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + void init_serialdevice_ports(void); +#endif + void update(); bool enabled(void) const { return _enable != 0; }; @@ -138,6 +150,10 @@ class AP_Scripting command_block_list *mavlink_command_block_list; HAL_Semaphore mavlink_command_block_list_sem; + #if AP_SCRIPTING_SERIALDEVICE_ENABLED + AP_Scripting_SerialDevice _serialdevice; + #endif + private: void thread(void); // main script execution thread diff --git a/libraries/AP_Scripting/AP_Scripting_SerialAccess.cpp b/libraries/AP_Scripting/AP_Scripting_SerialAccess.cpp new file mode 100644 index 0000000000000..f82ef3a127e9e --- /dev/null +++ b/libraries/AP_Scripting/AP_Scripting_SerialAccess.cpp @@ -0,0 +1,72 @@ +/* + generic object to allow a script to use a serial driver stream from both + driver and device perspectives + */ + +#include "AP_Scripting_config.h" +#include "AP_Scripting.h" +#include "AP_Scripting_SerialAccess.h" + +#if AP_SCRIPTING_ENABLED + +#if AP_SCRIPTING_SERIALDEVICE_ENABLED +#define check_is_device_port() (is_device_port) +#define ON_DEVICE_PORT(func, ...) (((AP_Scripting_SerialDevice::Port*)stream)->device_##func (__VA_ARGS__)) +#else +#define check_is_device_port() (false) +#define ON_DEVICE_PORT(...) (0) // not executed +#endif + +void AP_Scripting_SerialAccess::begin(uint32_t baud) +{ + if (!check_is_device_port()) { + stream->begin(baud); + } +} + +size_t AP_Scripting_SerialAccess::write(uint8_t c) +{ + return write(&c, 1); +} + +size_t AP_Scripting_SerialAccess::write(const uint8_t *buffer, size_t size) +{ + if (!check_is_device_port()) { + return stream->write(buffer, size); + } + return ON_DEVICE_PORT(write, buffer, size); +} + +int16_t AP_Scripting_SerialAccess::read(void) +{ + uint8_t c; + if (read(&c, 1) != 1) { + return -1; + } + return c; +} + +ssize_t AP_Scripting_SerialAccess::read(uint8_t* buffer, uint16_t count) +{ + if (!check_is_device_port()) { + return stream->read(buffer, count); + } + return ON_DEVICE_PORT(read, buffer, count); +} + +uint32_t AP_Scripting_SerialAccess::available(void) +{ + if (!check_is_device_port()) { + return stream->available(); + } + return ON_DEVICE_PORT(available); +} + +void AP_Scripting_SerialAccess::set_flow_control(enum AP_HAL::UARTDriver::flow_control fcs) +{ + if (!check_is_device_port()) { + stream->set_flow_control(fcs); + } +} + +#endif // AP_SCRIPTING_ENABLED diff --git a/libraries/AP_Scripting/AP_Scripting_SerialAccess.h b/libraries/AP_Scripting/AP_Scripting_SerialAccess.h new file mode 100644 index 0000000000000..7231359c81855 --- /dev/null +++ b/libraries/AP_Scripting/AP_Scripting_SerialAccess.h @@ -0,0 +1,33 @@ +#pragma once + +#include "AP_Scripting_config.h" +#include "AP_Scripting.h" + +#include + +class AP_Scripting_SerialAccess { +public: + /* Do not allow copies */ + CLASS_NO_COPY(AP_Scripting_SerialAccess); + + AP_Scripting_SerialAccess() {} + + void begin(uint32_t baud); + + size_t write(uint8_t c); + size_t write(const uint8_t *buffer, size_t size); + + int16_t read(void); + ssize_t read(uint8_t *buffer, uint16_t count); + + uint32_t available(void); + + void set_flow_control(enum AP_HAL::UARTDriver::flow_control fcs); + + AP_HAL::UARTDriver *stream; +#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + bool is_device_port; +#endif +#endif +}; diff --git a/libraries/AP_Scripting/AP_Scripting_SerialDevice.cpp b/libraries/AP_Scripting/AP_Scripting_SerialDevice.cpp new file mode 100644 index 0000000000000..39412246d5539 --- /dev/null +++ b/libraries/AP_Scripting/AP_Scripting_SerialDevice.cpp @@ -0,0 +1,160 @@ +/* + port for a script to access from a device perspective + */ + +#include "AP_Scripting_config.h" +#include "AP_Scripting.h" + +#if AP_SCRIPTING_ENABLED && AP_SCRIPTING_SERIALDEVICE_ENABLED + +#include +#include + +#ifndef AP_SCRIPTING_SERIALDEVICE_MIN_TXSIZE +#define AP_SCRIPTING_SERIALDEVICE_MIN_TXSIZE 2048 +#endif + +#ifndef AP_SCRIPTING_SERIALDEVICE_MIN_RXSIZE +#define AP_SCRIPTING_SERIALDEVICE_MIN_RXSIZE 2048 +#endif + +/* + initialise scripting serial ports +*/ +void AP_Scripting_SerialDevice::init(void) +{ + if (enable == 0) { + return; + } + + for (uint8_t i=0; iclear(); + } + if (writebuffer) { + writebuffer->clear(); + } +} + +size_t AP_Scripting_SerialDevice::Port::device_write(const uint8_t *buffer, size_t size) +{ + WITH_SEMAPHORE(sem); + if (readbuffer) { + return readbuffer->write(buffer, size); + } + return 0; +} + +ssize_t AP_Scripting_SerialDevice::Port::device_read(uint8_t *buffer, uint16_t count) +{ + WITH_SEMAPHORE(sem); + if (writebuffer) { + return writebuffer->read(buffer, count); + } + return 0; +} + +uint32_t AP_Scripting_SerialDevice::Port::device_available(void) +{ + WITH_SEMAPHORE(sem); + if (writebuffer) { + return writebuffer->available(); + } + return 0; +} + +/* + available space in outgoing buffer + */ +uint32_t AP_Scripting_SerialDevice::Port::txspace(void) +{ + WITH_SEMAPHORE(sem); + return writebuffer != nullptr ? writebuffer->space() : 0; +} + +void AP_Scripting_SerialDevice::Port::_begin(uint32_t b, uint16_t rxS, uint16_t txS) +{ + rxS = MAX(rxS, AP_SCRIPTING_SERIALDEVICE_MIN_RXSIZE); + txS = MAX(txS, AP_SCRIPTING_SERIALDEVICE_MIN_TXSIZE); + init_buffers(rxS, txS); +} + +size_t AP_Scripting_SerialDevice::Port::_write(const uint8_t *buffer, size_t size) +{ + WITH_SEMAPHORE(sem); + return writebuffer != nullptr ? writebuffer->write(buffer, size) : 0; +} + +ssize_t AP_Scripting_SerialDevice::Port::_read(uint8_t *buffer, uint16_t count) +{ + WITH_SEMAPHORE(sem); + return readbuffer != nullptr ? readbuffer->read(buffer, count) : -1; +} + +uint32_t AP_Scripting_SerialDevice::Port::_available() +{ + WITH_SEMAPHORE(sem); + return readbuffer != nullptr ? readbuffer->available() : 0; +} + + +bool AP_Scripting_SerialDevice::Port::_discard_input() +{ + WITH_SEMAPHORE(sem); + if (readbuffer != nullptr) { + readbuffer->clear(); + } + return true; +} + +/* + initialise read/write buffers + */ +bool AP_Scripting_SerialDevice::Port::init_buffers(const uint32_t size_rx, const uint32_t size_tx) +{ + if (size_tx == last_size_tx && + size_rx == last_size_rx) { + return true; + } + WITH_SEMAPHORE(sem); + if (readbuffer == nullptr) { + readbuffer = NEW_NOTHROW ByteBuffer(size_rx); + } else { + readbuffer->set_size_best(size_rx); + } + if (writebuffer == nullptr) { + writebuffer = NEW_NOTHROW ByteBuffer(size_tx); + } else { + writebuffer->set_size_best(size_tx); + } + last_size_rx = size_rx; + last_size_tx = size_tx; + return readbuffer != nullptr && writebuffer != nullptr; +} + +#endif // AP_SCRIPTING_ENABLED && AP_SCRIPTING_SERIALDEVICE_ENABLED diff --git a/libraries/AP_Scripting/AP_Scripting_SerialDevice.h b/libraries/AP_Scripting/AP_Scripting_SerialDevice.h new file mode 100644 index 0000000000000..979405a8b3290 --- /dev/null +++ b/libraries/AP_Scripting/AP_Scripting_SerialDevice.h @@ -0,0 +1,63 @@ +#pragma once + +#include + +#ifndef AP_SCRIPTING_SERIALDEVICE_NUM_PORTS +#define AP_SCRIPTING_SERIALDEVICE_NUM_PORTS 3 +#endif + +class AP_Scripting; + +class AP_Scripting_SerialDevice +{ +public: + /* Do not allow copies */ + CLASS_NO_COPY(AP_Scripting_SerialDevice); + + AP_Scripting_SerialDevice() {} + + AP_Int8 enable; + + void init(void); + void clear(void); + +public: + class Port : public AP_SerialManager::RegisteredPort { + public: + friend class AP_Scripting_SerialDevice; + void init(void); + void clear(void); + + size_t device_write(const uint8_t *buffer, size_t size); + ssize_t device_read(uint8_t *buffer, uint16_t count); + uint32_t device_available(void); + + private: + bool is_initialized() override { + return true; + } + bool tx_pending() override { + return false; + } + + bool init_buffers(const uint32_t size_rx, const uint32_t size_tx); + + uint32_t txspace() override; + void _begin(uint32_t b, uint16_t rxS, uint16_t txS) override; + size_t _write(const uint8_t *buffer, size_t size) override; + ssize_t _read(uint8_t *buffer, uint16_t count) override; + uint32_t _available() override; + void _end() override {} + void _flush() override {} + bool _discard_input() override; + + ByteBuffer *readbuffer; + ByteBuffer *writebuffer; + uint32_t last_size_tx; + uint32_t last_size_rx; + + HAL_Semaphore sem; + }; + + Port ports[AP_SCRIPTING_SERIALDEVICE_NUM_PORTS]; +}; diff --git a/libraries/AP_Scripting/docs/docs.lua b/libraries/AP_Scripting/docs/docs.lua index 7171b241c5296..f60cdf7712a57 100644 --- a/libraries/AP_Scripting/docs/docs.lua +++ b/libraries/AP_Scripting/docs/docs.lua @@ -1229,41 +1229,46 @@ function AP_HAL__I2CDevice_ud:write_register(register_num, value) end function AP_HAL__I2CDevice_ud:set_retries(retries) end --- Serial driver object ----@class (exact) AP_HAL__UARTDriver_ud -local AP_HAL__UARTDriver_ud = {} +-- Serial port access object +---@class (exact) AP_Scripting_SerialAccess_ud +local AP_Scripting_SerialAccess_ud = {} --- Set flow control option for serial port ----@param flow_control_setting integer ----| '0' # disabled ----| '1' # enabled ----| '2' # auto -function AP_HAL__UARTDriver_ud:set_flow_control(flow_control_setting) end - --- Returns number of available bytes to read. ----@return uint32_t_ud -function AP_HAL__UARTDriver_ud:available() end +-- Start serial port with the given baud rate (no effect for device ports) +---@param baud_rate uint32_t_ud|integer|number +function AP_Scripting_SerialAccess_ud:begin(baud_rate) end -- Writes a single byte ---@param value integer -- byte to write ---@return uint32_t_ud -- 1 if success else 0 -function AP_HAL__UARTDriver_ud:write(value) end +function AP_Scripting_SerialAccess_ud:write(value) end --- Read a single byte from the serial port ----@return integer -- byte, -1 if not available -function AP_HAL__UARTDriver_ud:read() end +-- Writes a string. The number of bytes actually written, i.e. the length of the +-- written prefix of the string, is returned. It may be 0 up to the length of +-- the string. +---@param data string -- string of bytes to write +---@return integer -- number of bytes actually written, which may be 0 +function AP_Scripting_SerialAccess_ud:writestring(data) end --- Start serial port with given baud rate ----@param baud_rate uint32_t_ud|integer|number -function AP_HAL__UARTDriver_ud:begin(baud_rate) end +-- Reads a single byte from the serial port +---@return integer -- byte, -1 if error or none available +function AP_Scripting_SerialAccess_ud:read() end ---[[ - read count bytes from a uart and return as a lua string. Note - that the returned string can be shorter than the requested length ---]] ----@param count integer ----@return string|nil -function AP_HAL__UARTDriver_ud:readstring(count) end +-- Reads up to `count` bytes and returns the bytes read as a string. No bytes +-- may be read, in which case a 0-length string is returned. +---@param count integer -- maximum number of bytes to read +---@return string|nil -- bytes actually read, which may be 0-length, or nil on error +function AP_Scripting_SerialAccess_ud:readstring(count) end + +-- Returns number of available bytes to read. +---@return uint32_t_ud +function AP_Scripting_SerialAccess_ud:available() end + +-- Set flow control option for serial port (no effect for device ports) +---@param flow_control_setting integer +---| '0' # disabled +---| '1' # enabled +---| '2' # auto +function AP_Scripting_SerialAccess_ud:set_flow_control(flow_control_setting) end -- desc @@ -2102,13 +2107,24 @@ function baro:healthy(instance) end -- Serial ports serial = {} --- Returns the UART instance that allows connections from scripts (those with SERIALx_PROTOCOL = 28). --- For instance = 0, returns first such UART, second for instance = 1, and so on. --- If such an instance is not found, returns nil. ----@param instance integer -- the 0-based index of the UART instance to return. ----@return AP_HAL__UARTDriver_ud|nil -- the requested UART instance available for scripting, or nil if none. +-- Returns a serial access object that allows a script to interface with a +-- device over a port set to protocol 28 (Scripting) (e.g. SERIALx_PROTOCOL). +-- Instance 0 is the first such port, instance 1 the second, and so on. If the +-- requested instance is not found, returns nil. +---@param instance integer -- 0-based index of the Scripting port to access +---@return AP_Scripting_SerialAccess_ud|nil -- access object for that instance, or nil if not found function serial:find_serial(instance) end +-- Returns a serial access object that allows a script to simulate a device +-- attached via a specific protocol. The device protocol is configured by +-- SCR_SDEVx_PROTO. Instance 0 is the first such protocol, instance 1 the +-- second, and so on. If the requested instance is not found, or SCR_SDEV_EN is +-- disabled, returns nil. +---@param protocol integer -- protocol to access +---@param instance integer -- 0-based index of the protocol instance to access +---@return AP_Scripting_SerialAccess_ud|nil -- access object for that instance, or nil if not found +function serial:find_simulated_device(protocol, instance) end + -- desc rc = {} diff --git a/libraries/AP_Scripting/examples/gps_synth.lua b/libraries/AP_Scripting/examples/gps_synth.lua new file mode 100644 index 0000000000000..ef3ce6b00b5a0 --- /dev/null +++ b/libraries/AP_Scripting/examples/gps_synth.lua @@ -0,0 +1,170 @@ +-- get GPS data from ardupilot's native bindings then resynthesize into a +-- virtual NMEA GPS and feed back through the serial device sim bindings. +-- demonstrates the bindings and provides the opportunity for script-controlled +-- tampering and other such activities. + +-- parameters: +-- SCR_ENABLE 1 +-- SCR_SDEV_EN 1 +-- SCR_SDEV1_PROTO 5 +-- SERIAL3_PROTOCOL 5 +-- SERIAL4_PROTOCOL -1 +-- GPS2_TYPE 5 +-- GPS_PRIMARY 1 +-- GPS_AUTO_SWITCH 0 + +local ser_device = serial:find_simulated_device(5, 0) +if not ser_device then + error("SCR_SDEV_EN must be 1 and SCR_SDEVn_PROTO must be 5") +end + +function convert_coord(coord, dir) + -- convert ardupilot degrees*1e7 to NMEA degrees + decimal minutes + dir. + -- the first character of dir is used if the coordinate is positive, + -- the second if negative. + + -- handle sign + if coord < 0 then + coord = -coord + dir = dir:sub(2, 2) + else + dir = dir:sub(1, 1) + end + + local degrees = coord // 10000000 -- integer divide + coord = coord - (degrees * 10000000) -- remove that portion + local minutes = coord * (60/10000000) -- float divide + + return ("%03d%08.5f,%s"):format(degrees, minutes, dir) +end + +function convert_time(time_week, time_week_ms) + -- convert ardupilot GPS time to NMEA UTC date/time strings + + -- GPS week 1095 starts on Dec 31 2000 + local seconds_per_week = uint32_t(86400*7) + timestamp_s = uint32_t(time_week - 1095)*seconds_per_week + -- subtract one day to get to Jan 1 2001, then 18 additional seconds to + -- account for the GPS to UTC leap second induced offset + timestamp_s = timestamp_s - uint32_t(86400 + 18) + -- add in time within the week + timestamp_s = timestamp_s + (time_week_ms/uint32_t(1000)) + + timestamp_s = timestamp_s:toint() -- seconds since Jan 1 2001 + + local ts_year = 2001 + local day_seconds = 86400 + while true do + local year_seconds = day_seconds * ((ts_year % 4 == 0) and 366 or 365) + if timestamp_s >= year_seconds then + timestamp_s = timestamp_s - year_seconds + ts_year = ts_year + 1 + else + break + end + end + + local month_days = {31, (ts_year % 4 == 0) and 29 or 28, + 31, 30, 31, 30, 31, 31, 30, 31, 30, 31} + + local ts_month = 1 + for _, md in ipairs(month_days) do + local month_seconds = 86400 * md + if timestamp_s >= month_seconds then + timestamp_s = timestamp_s - month_seconds + ts_month = ts_month + 1 + else + break + end + end + + local ts_day = 1+(timestamp_s // 86400) + timestamp_s = timestamp_s % 86400 + + local ts_hour = timestamp_s // 3600 + timestamp_s = timestamp_s % 3600 + + local ts_minute = timestamp_s // 60 + local ts_second = timestamp_s % 60 + + local date = ("%02d%02d%02d"):format(ts_year-2000, ts_month, ts_day) + local time = ("%02d%02d%02d.%01d"):format(ts_hour, ts_minute, ts_second, + (time_week_ms % 1000):toint()//100) + + return date, time +end + +function get_gps_data(instance) + -- get GPS data from ardupilot scripting bindings in native format + local data = { + hdop = gps:get_hdop(instance), + time_week_ms = gps:time_week_ms(instance), + time_week = gps:time_week(instance), + sats = gps:num_sats(instance), + crs = gps:ground_course(instance), + spd = gps:ground_speed(instance), + loc = gps:location(instance), + status = gps:status(instance), + } + if data.status < gps.GPS_OK_FIX_3D then + return nil -- don't bother with invalid data + end + return data +end + +function arrange_nmea(data) + -- convert ardupilot data entries to NMEA-compatible format + local ts_date, ts_time = convert_time(data.time_week, data.time_week_ms) + + return { + time = ts_time, + lat = convert_coord(data.loc:lat(), "NS"), + lng = convert_coord(data.loc:lng(), "EW"), + spd = data.spd / 0.514, -- m/s to knots + crs = data.crs, -- degrees + date = ts_date, + sats = data.sats, + hdop = data.hdop, + alt = data.loc:alt()/100, + } +end + +function wrap_nmea(msg) + -- compute checksum and add header and footer + local checksum = 0 + for i = 1,#msg do + checksum = checksum ~ msg:byte(i, i) + end + + return ("$%s*%02X\r\n"):format(msg, checksum) +end + +function format_nmea(data) + -- format data into complete NMEA sentences + local rmc_raw = ("GPRMC,%s,A,%s,%s,%03f,%03f,%s,000.0,E"):format( + data.time, data.lat, data.lng, data.spd, data.crs, data.date) + + local gga_raw = ("GPGGA,%s,%s,%s,1,%02d,%05.2f,%06.2f,M,0,M,,"):format( + data.time, data.lat, data.lng, data.sats, data.hdop/100, data.alt) + + return wrap_nmea(rmc_raw), wrap_nmea(gga_raw) +end + +function update() + -- get data from first instance (we are the second) + local ardu_data = get_gps_data(0) + + if ardu_data then + local nmea_data = arrange_nmea(ardu_data) + local rmc, gga = format_nmea(nmea_data) + + if ser_device:writestring(rmc) ~= #rmc + or ser_device:writestring(gga) ~= #gga then + error("overflow, ardupilot is not processing the data, check config!") + end + end + + return update, 200 -- 5Hz like a real GPS +end + +return update() diff --git a/libraries/AP_Scripting/generator/description/bindings.desc b/libraries/AP_Scripting/generator/description/bindings.desc index 40910361296c5..46e9a1f6d2125 100644 --- a/libraries/AP_Scripting/generator/description/bindings.desc +++ b/libraries/AP_Scripting/generator/description/bindings.desc @@ -392,18 +392,21 @@ singleton RC_Channels method lua_rc_channel RC_Channel uint8_t 1 NUM_RC_CHANNELS singleton RC_Channels method lua_rc_channel rename get_channel singleton RC_Channels method get_aux_cached boolean RC_Channel::AUX_FUNC'enum 0 UINT16_MAX uint8_t'Null -include AP_SerialManager/AP_SerialManager.h - -ap_object AP_HAL::UARTDriver method begin void uint32_t 1U UINT32_MAX -ap_object AP_HAL::UARTDriver method read int16_t -ap_object AP_HAL::UARTDriver manual readstring AP_HAL__UARTDriver_readstring 1 1 -ap_object AP_HAL::UARTDriver method write uint32_t uint8_t'skip_check -ap_object AP_HAL::UARTDriver method available uint32_t -ap_object AP_HAL::UARTDriver method set_flow_control void AP_HAL::UARTDriver::flow_control'enum AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE AP_HAL::UARTDriver::FLOW_CONTROL_RTS_DE - -singleton AP_SerialManager rename serial -singleton AP_SerialManager depends HAL_GCS_ENABLED -singleton AP_SerialManager method find_serial AP_HAL::UARTDriver AP_SerialManager::SerialProtocol_Scripting'literal uint8_t'skip_check +include AP_Scripting/AP_Scripting_SerialAccess.h +-- don't let user create access objects +userdata AP_Scripting_SerialAccess creation null -1 +userdata AP_Scripting_SerialAccess method begin void uint32_t 1U UINT32_MAX +userdata AP_Scripting_SerialAccess method write uint32_t uint8_t'skip_check +userdata AP_Scripting_SerialAccess manual writestring lua_serial_writestring 1 1 +userdata AP_Scripting_SerialAccess method read int16_t +userdata AP_Scripting_SerialAccess manual readstring lua_serial_readstring 1 1 +userdata AP_Scripting_SerialAccess method available uint32_t +userdata AP_Scripting_SerialAccess method set_flow_control void AP_HAL::UARTDriver::flow_control'enum AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE AP_HAL::UARTDriver::FLOW_CONTROL_RTS_DE + +-- serial is not a real C++ type here, but its name never gets used in C++ as we only have manual methods +singleton serial depends HAL_GCS_ENABLED +singleton serial manual find_serial lua_serial_find_serial 1 1 +singleton serial manual find_simulated_device lua_serial_find_simulated_device 2 1 depends AP_SCRIPTING_SERIALDEVICE_ENABLED include AP_Baro/AP_Baro.h singleton AP_Baro depends (!defined(HAL_BUILD_AP_PERIPH) || defined(HAL_PERIPH_ENABLE_BARO)) diff --git a/libraries/AP_Scripting/generator/src/main.c b/libraries/AP_Scripting/generator/src/main.c index e7758e7a7c286..b2f4e300103d4 100644 --- a/libraries/AP_Scripting/generator/src/main.c +++ b/libraries/AP_Scripting/generator/src/main.c @@ -2414,11 +2414,13 @@ void emit_index(struct userdata *head) { struct method_alias *alias = node->method_aliases; while(alias) { + start_dependency(source, alias->dependency); if (alias->type == ALIAS_TYPE_MANUAL) { fprintf(source, " {\"%s\", %s},\n", alias->alias, alias->name); } else if (alias->type == ALIAS_TYPE_NONE) { fprintf(source, " {\"%s\", %s_%s},\n", alias->alias, node->sanatized_name, alias->name); } + end_dependency(source, alias->dependency); alias = alias->next; } @@ -2568,8 +2570,8 @@ void emit_sandbox(void) { // Dont expose creation function for all read only items int expose_creation = FALSE; if (data->creation || data->methods) { - // Custom creation or methods - expose_creation = TRUE; + // Custom creation or methods, if not specifically disabled + expose_creation = !(data->creation && data->creation_args == -1); } else { // Feilds only struct userdata_field * field = data->fields; @@ -2866,7 +2868,8 @@ void emit_docs(struct userdata *node, int is_userdata, int emit_creation) { // local userdata fprintf(docs, "local %s = {}\n\n", name); - if (emit_creation) { + int creation_disabled = (node->creation && node->creation_args == -1); + if (emit_creation && (!node->creation || !creation_disabled)) { // creation function if (node->creation != NULL) { for (int i = 0; i < node->creation_args; ++i) { diff --git a/libraries/AP_Scripting/lua_bindings.cpp b/libraries/AP_Scripting/lua_bindings.cpp index de82efd87013e..67a589b6d7c47 100644 --- a/libraries/AP_Scripting/lua_bindings.cpp +++ b/libraries/AP_Scripting/lua_bindings.cpp @@ -649,30 +649,6 @@ int AP_HAL__I2CDevice_read_registers(lua_State *L) { return success; } -int AP_HAL__UARTDriver_readstring(lua_State *L) { - binding_argcheck(L, 2); - - AP_HAL::UARTDriver * ud = *check_AP_HAL__UARTDriver(L, 1); - - const uint16_t count = get_uint16_t(L, 2); - uint8_t *data = (uint8_t*)malloc(count); - if (data == nullptr) { - return 0; - } - - const auto ret = ud->read(data, count); - if (ret < 0) { - free(data); - return 0; - } - - // push to lua string - lua_pushlstring(L, (const char *)data, ret); - free(data); - - return 1; -} - #if AP_SCRIPTING_CAN_SENSOR_ENABLED int lua_get_CAN_device(lua_State *L) { @@ -737,6 +713,111 @@ int lua_get_CAN_device2(lua_State *L) { } #endif // AP_SCRIPTING_CAN_SENSOR_ENABLED +#if HAL_GCS_ENABLED +int lua_serial_find_serial(lua_State *L) { + // Allow : and . access + const int arg_offset = (luaL_testudata(L, 1, "serial") != NULL) ? 1 : 0; + + binding_argcheck(L, 1 + arg_offset); + + uint8_t instance = get_uint8_t(L, 1 + arg_offset); + + AP_SerialManager *mgr = &AP::serialmanager(); + AP_HAL::UARTDriver *driver_stream = mgr->find_serial( + AP_SerialManager::SerialProtocol_Scripting, instance); + + if (driver_stream == nullptr) { // not found + return 0; + } + + new_AP_Scripting_SerialAccess(L); + AP_Scripting_SerialAccess *port = check_AP_Scripting_SerialAccess(L, -1); + port->stream = driver_stream; +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + port->is_device_port = false; +#endif + + return 1; +} +#endif // HAL_GCS_ENABLED + +#if AP_SCRIPTING_SERIALDEVICE_ENABLED +int lua_serial_find_simulated_device(lua_State *L) { + // Allow : and . access + const int arg_offset = (luaL_testudata(L, 1, "serial") != NULL) ? 1 : 0; + + binding_argcheck(L, 2 + arg_offset); + + const int8_t protocol = (int8_t)get_uint32(L, 1 + arg_offset, 0, 127); + uint32_t instance = get_uint16_t(L, 2 + arg_offset); + + auto *scripting = AP::scripting(); + AP_Scripting_SerialDevice::Port *device_stream = nullptr; + + for (auto &port : scripting->_serialdevice.ports) { + if (port.state.protocol == protocol) { + if (instance-- == 0) { + device_stream = &port; + break; + } + } + } + + if (!scripting->_serialdevice.enable || device_stream == nullptr) { + // serial devices as a whole are disabled, or port not found + return 0; + } + + new_AP_Scripting_SerialAccess(L); + AP_Scripting_SerialAccess *port = check_AP_Scripting_SerialAccess(L, -1); + port->stream = device_stream; + port->is_device_port = true; + + return 1; +} +#endif // AP_SCRIPTING_SERIALDEVICE_ENABLED + +int lua_serial_writestring(lua_State *L) +{ + binding_argcheck(L, 2); + + AP_Scripting_SerialAccess * port = check_AP_Scripting_SerialAccess(L, 1); + + // get the bytes the user wants to write, along with their length + size_t req_bytes; + const char *data = lua_tolstring(L, 2, &req_bytes); + + // write up to that number of bytes + const uint32_t written_bytes = port->write((const uint8_t*)data, req_bytes); + + // return the number of bytes that were actually written + lua_pushinteger(L, written_bytes); + + return 1; +} + +int lua_serial_readstring(lua_State *L) { + binding_argcheck(L, 2); + + AP_Scripting_SerialAccess * port = check_AP_Scripting_SerialAccess(L, 1); + + // create a buffer sized to hold the number of bytes the user wants to read + luaL_Buffer b; + const uint16_t req_bytes = get_uint16_t(L, 2); + uint8_t *data = (uint8_t *)luaL_buffinitsize(L, &b, req_bytes); + + // read up to that number of bytes + const ssize_t read_bytes = port->read(data, req_bytes); + if (read_bytes < 0) { + return 0; // error, return nil + } + + // push the buffer as a string, truncated to the number of bytes actually read + luaL_pushresultsize(&b, read_bytes); + + return 1; +} + /* directory listing, return table of files in a directory */ diff --git a/libraries/AP_Scripting/lua_bindings.h b/libraries/AP_Scripting/lua_bindings.h index add40faa0a7bd..53b5f4b5ae656 100644 --- a/libraries/AP_Scripting/lua_bindings.h +++ b/libraries/AP_Scripting/lua_bindings.h @@ -8,9 +8,12 @@ int lua_mission_receive(lua_State *L); int AP_Logger_Write(lua_State *L); int lua_get_i2c_device(lua_State *L); int AP_HAL__I2CDevice_read_registers(lua_State *L); -int AP_HAL__UARTDriver_readstring(lua_State *L); int lua_get_CAN_device(lua_State *L); int lua_get_CAN_device2(lua_State *L); +int lua_serial_find_serial(lua_State *L); +int lua_serial_find_simulated_device(lua_State *L); +int lua_serial_writestring(lua_State *L); +int lua_serial_readstring(lua_State *L); int lua_dirlist(lua_State *L); int lua_removefile(lua_State *L); int SRV_Channels_get_safety_state(lua_State *L); diff --git a/libraries/AP_Scripting/tests/serial_loopback.lua b/libraries/AP_Scripting/tests/serial_loopback.lua new file mode 100644 index 0000000000000..9893c8f7d8e47 --- /dev/null +++ b/libraries/AP_Scripting/tests/serial_loopback.lua @@ -0,0 +1,36 @@ +local ser_driver = serial:find_serial(0) +local ser_device = serial:find_simulated_device(28, 0) + +if ser_driver == nil or ser_device == nil then + error("bad config") +end + +ser_driver:begin(115200) -- baud rate does not matter + +function test_driver_to_device() + local msg_send = "hello device" + local num_sent = 0 + for ci = 1,#msg_send do + num_sent = num_sent + ser_driver:write(msg_send:byte(ci, ci)):toint() + end + local msg_recv = ser_device:readstring(#msg_send) + if msg_send == msg_recv and num_sent == #msg_send then + gcs:send_text(6, "driver -> device good") + end +end + +function test_device_to_driver() + local msg_send = "hello driver" + local num_sent = ser_device:writestring(msg_send) + local msg_recv = ser_driver:readstring(#msg_send) + if msg_send == msg_recv and num_sent == #msg_send then + gcs:send_text(6, "device -> driver good") + end +end + +function update() + test_driver_to_device() + test_device_to_driver() +end + +return update() diff --git a/libraries/AP_SerialManager/AP_SerialManager_config.h b/libraries/AP_SerialManager/AP_SerialManager_config.h index fb25527a8ff7f..74ed01243bec2 100644 --- a/libraries/AP_SerialManager/AP_SerialManager_config.h +++ b/libraries/AP_SerialManager/AP_SerialManager_config.h @@ -71,6 +71,9 @@ #define AP_SERIALMANAGER_CAN_D1_PORT_1 41 // CAN_D1_UC_S1_* #define AP_SERIALMANAGER_CAN_D2_PORT_1 51 // CAN_D2_UC_S1_* +// serial device simulation ports registered by AP_Scripting will use IDs starting at 61 for the first port +#define AP_SERIALMANAGER_SCR_PORT_1 61 // SCR_SDEV1_* + // console default baud rates and buffer sizes #ifdef DEFAULT_SERIAL0_BAUD #define AP_SERIALMANAGER_CONSOLE_BAUD DEFAULT_SERIAL0_BAUD diff --git a/libraries/AP_Vehicle/AP_Vehicle.cpp b/libraries/AP_Vehicle/AP_Vehicle.cpp index 6b13e2b409d9b..4ad2e0ae5a798 100644 --- a/libraries/AP_Vehicle/AP_Vehicle.cpp +++ b/libraries/AP_Vehicle/AP_Vehicle.cpp @@ -367,6 +367,14 @@ void AP_Vehicle::setup() networking.init(); #endif +#if AP_SCRIPTING_ENABLED +#if AP_SCRIPTING_SERIALDEVICE_ENABLED + // must be done now so ports are registered and drivers get set up properly + // (in particular mavlink which checks during init_ardupilot()) + scripting.init_serialdevice_ports(); +#endif +#endif + #if AP_SCHEDULER_ENABLED // Register scheduler_delay_cb, which will run anytime you have // more than 5ms remaining in your call to hal.scheduler->delay