V-rep学习笔记：串口操作

Posted 2020-10-04

　　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?