diff --git a/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_SERIAL_CONTROL.lua b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_SERIAL_CONTROL.lua new file mode 100644 index 0000000000..e33110f7b7 --- /dev/null +++ b/libraries/AP_Scripting/modules/MAVLink/mavlink_msg_SERIAL_CONTROL.lua @@ -0,0 +1,11 @@ +local SERIAL_CONTROL = {} +SERIAL_CONTROL.id = 126 +SERIAL_CONTROL.fields = { + { "baudrate", " 0 then + -- encode the message and store it for transmission + local msg = self._tx_template + msg.count = self._tx_count + _, self._tx_msg = mavlink_msgs.encode("SERIAL_CONTROL", msg) + self._tx_count = 0 + end + + if self._tx_msg then -- message to send? + if mavlink.send_chan(self._chan, SERIAL_CONTROL.id, self._tx_msg) then + self._tx_msg = nil -- successfully sent + end + end +end + +function mavport:read() + if not self._rx_buf then + self:_receive() + if not self._rx_buf then return -1 end + end + + local b = self._rx_buf + local pos = self._rx_pos + local c = b[pos] + + self._rx_pos = pos + 1 + if pos == #b then + self._rx_buf = nil + end + + return c +end + +function mavport:readstring(count) + local avail = self:available() -- also fills rx buf + if avail == 0 then return "" end + if count > avail then count = avail end + + local b = self._rx_buf + local pos = self._rx_pos + local s = string.char(table.unpack(b, pos, pos+count-1)) + + pos = pos + count + if pos > #b then + self._rx_buf = nil + end + self._rx_pos = pos + + return s +end + +function mavport:_receive() + local msg, chan + while true do + msg, chan = mavlink.receive_chan() + if not msg then return end -- no new messages + + -- decode message and handle if it's for us + msg = mavlink_msgs.decode(msg, msg_map) + if msg.device == 10 then -- for the dev shell? + self._chan = chan -- reply on the same channel + break + end + end + + local data = msg.data + local count = msg.count + -- remove trailing nulls, they shouldn't happen but they do + while data[count] == 0 do + count = count - 1 + end + + -- store received bytes + if count > 0 then + if count < SERIAL_CONTROL_DATA_LEN then -- remove trailing junk + data = table.move(data, 1, count, 1, {}) + end + self._rx_buf = data + self._rx_pos = 1 + end +end + +function mavport:available() + if not self._rx_buf then + self:_receive() + if not self._rx_buf then return 0 end + end + + return #self._rx_buf - self._rx_pos + 1 +end + +-- for completeness +function mavport.set_flow_control(_, _) +end + +return mavport