V-rep學習筆記：串口操作

　　VREP Regular API提供了串口操作的相關函數，可以對串口進行打開、關閉和讀寫：

　　下面使用一款淘寶上常見的AHRS（Attitude and heading reference system，航姿參考系統）模塊來驅動VREP中的虛擬模型，控制其姿態。VREP通過串口讀取傳感器實時發送的數據並進行解析。

 

　　傳感器通過串口發送2種數據：

1. 解算后的姿態角和氣壓高度等數據 
2. 原始的傳感器ADC數據（直接從傳感器讀取出來的測量值，沒有經過解算處理）

　　下面是VREP中以16進制顯示的接收到的串口數據：

 　　根據約定的通信協議，上位機要對接收到的字符串進行解析。關鍵的一步是要將2個byte型（8位）變量轉換為int16型（有符號整型變量，16位），因為角度、高度、溫度等數據都可以有正負。

　　計算機中的符號數有三種表示方法，即原碼、反碼和補碼。三種表示方法均有符號位和數值位兩部分，符號位都是用0表示“正”，用1表示“負”，而數值位，三種表示方法各不相同。

　　以0x8000為例，若用無符號數表達其值為32768，若用有符號數表達其值為-32768。 因為0x8000最高位為1是負數，其代表的值為-(0x7FFF + 1) = -(0x8000) = - 32768（負數的補碼求原碼和負數的原碼求補碼的方法一樣：除符號位外，每位求反，末位加一）

• 原碼就是符號位加上真值的絕對值，即用第一位表示符號，其余位表示值。比如：

[+1]_原 = 0000 0001

[-1]_原 = 1000 0001

• 正數的反碼是其本身，負數的反碼是在其原碼的基礎上，符號位不變，其余各個位取反：

[+1] = [00000001]_原 = [00000001]_反

[-1] = [10000001]_原 = [11111110]_反

• 正數的補碼就是其本身，負數的補碼是在其原碼的基礎上，符號位不變，其余各位取反后+1. (即在反碼的基礎上+1)：

[+1] = [00000001]_原 = [00000001]_反 = [00000001]_補

[-1] = [10000001]_原 = [11111110]_反 = [11111111]_補

　　補碼運算的規則如下。采用補碼表示數，在計算機的加、減法運算中，不必判斷數的正負，只要符號位參加運算就能自動得到正確的結果。

[ x + y ]_補  =  x_補  +  y_補

[ x - y ]_補   =  x_補  +  [-y]_補

 

　　將兩個2個byte數據轉換為int16型的函數如下：

function make_int16(b1, b2)
　　assert (0 <= b1 and b1 <= 0xff)
　　assert (0 <= b1 and b2 <= 0xff)
　　local mask = (1 << 15)
　　local res  = (b1 << 8) | (b2 << 0)
　　return (res ~ mask) - mask
end

　　其中mask = (1000 0000 0000 0000)_b，將其與res進行異或操作（res~mask）只改變符號位，正數符號位變為1，負數符號位變為0。然后再減去mask，得到res的有符號數。

　　以0x81和0xC3為例：

　　res = 0x81<<8 | 0xC3 = (1000 0001 1100 0011)_b

　　res~mask = (0000 0001 1100 0011)_b

　　((res~mask) - mask)_補  =  (res~mask)_補+(-mask )_補 = (0000 0001 1100 0011)_b補 + (1000 0000 0000 0000)_b補 = (1000 0001 1100 0011)_b補 = -32317

　　Lua 5.3開始可以使用“<<”、“~”、“|”等運算符進行移位、取反、或等按位操作，而VREP 3.4中使用的是Lua 5.1還不支持按位操作運算符，可以下載合適的bitwise operation lib，下面使用了LuaBit來完成移位等操作。

 代碼如下：

require 'bit'

make_int16 = function(bh ,bl)

  assert (0 <= bh and bh <= 0xff)
  assert (0 <= bl and bl <= 0xff)
  local mask = bit.blshift(1, 15)                 -- mask = 1 << 15
  local res  = bit.bor(bit.blshift(bh,8), bl)     -- res = (bh << 8) | bl
  return bit.bxor(res, mask) - mask               -- (res ~ mask) - mask

end


HelicopterHandle = simGetObjectHandle("Helicopter")

--defining the serial port number
port = simGetScriptSimulationParameter(sim_handle_self,'serialPortNumber')
portNumber = "COM"..port
baudrate = 115200

-- Opens a serial port (RS-232) for communication
serial = simSerialOpen(portNumber, baudrate)


while (simGetSimulationState()~=sim_simulation_advancing_abouttostop) do

    -- Reads how many bytes are waiting to be read on a serial port
    --local number =simSerialCheck(serial)
    
    -- Reads from a previously opened serial port (RS-232). 
    local str = simSerialRead(serial, 44, true, '', 1)

    visibleString = ''
    if str ~= nil then 
        for i=1,#str,1 do
            visibleString = visibleString..string.format("%02X ", string.byte(str,i))
        end
        --simAddStatusbarMessage(visibleString)

        yaw    = make_int16(string.byte(str,5), string.byte(str,6))
        pitch  = make_int16(string.byte(str,7), string.byte(str,8))
        roll   = make_int16(string.byte(str,9), string.byte(str,10))

        simAddStatusbarMessage(string.format("Yaw:%.1f  Pitch:%.1f  Roll:%.1f", yaw*0.1,pitch*0.1,roll*0.1))
        simSetObjectOrientation(HelicopterHandle, -1, {math.pi/2-pitch*0.1*math.pi/180,-yaw*0.1*math.pi/180,roll*0.1*math.pi/180})
        
    end

end

-- Closes a serial port (RS-232). Port is automatically closed at simulation stop
simSerialClose(serial)

 

 

 

參考：
Bitwise Operators

原碼, 反碼, 補碼 詳解

Online Binary-Decimal Converter

How can I convert a pair of bytes into a signed 16-bit integer using Lua?