SEN0386 kommunikasjon micropython

Dette er en modul laget av dfrobot som kan returnere akselerasjon, rotasjonshastighet og vinkel i tre dimensjoner. Her en forklaring av input dataene.

Kommunikasjonsprotokoll: UART

Default baud: 9600

Antall bytes: 44

Data bits: 8

Paritet: Ingen

Stopp bits: 1

Bit rekkefølge: LSB

Spenning: 3V/5V

Måling

Dette er hentet ut fra ocilloscopet.

Totalt kommer det 4 pakker fra sensoren.

Pakke 1: Aksellerasjonsdata

Pakke 2: Rotasjonshastighet

Pakke 3: Retning

Den fjerde pakken er ikke i bruk, men leverer data.

Felles

Alle pakker starter med byte 0x55. Dette forteller at data pakkedata kommer.

Deretter kommer en pakke som forteller hva pakken inneholder. Hvis byte verdien er 0x51 så kommer det akselerasjonsdata, hvis den er 0x52 så er det rotasjonshastighet og 0x53 så er det retning.

Alle pakker er 11 bytes, nedenfor beskrives pakkene.

Akselerasjon

Blokker

Beregning

ax=((AxH<<8)|AxL)/32768*16*9.81
ay=((AyH<<8)|AyL)/32768*16*9.81
az=((AzH<<8)|AzL)/32768*16*9.81

Rotasjon

Blokker

Beregning

wx=((WxH<<8)|WxL)/32768*2000
wy=((WyH<<8)|WyL)/32768*2000
wz=((WzH<<8)|WzL)/32768*2000

Retning

Blokker

Kode

Koden kan lastes ned herfra: https://github.com/kjetilfjellheim/micropython-sen0386

def readSensorValues(uart):
    data = uart.read(BUFFER_SIZE)
    if data is not None and len(data) == BUFFER_SIZE:
        accPacket = data[ACCPACKET_BUFFER_START:ACCPACKET_BUFFER_END]
        angVelPacket = data[ANGVELPACKET_BUFFER_START:ANGVELPACKET_BUFFER_END]
        angPacket = data[ANGPACKET_BUFFER_START:ANGPACKET_BUFFER_END]
        ax, ay, az, wx, wy, wz, roll, pitch, yaw = handlePackets(accPacket, angVelPacket, angPacket)
        return ax, ay, az, wx, wy, wz, roll, pitch, yaw
    else:
        return UNDEFINED, UNDEFINED, UNDEFINED, UNDEFINED, UNDEFINED, UNDEFINED, UNDEFINED, UNDEFINED, UNDEFINED

Her leser vi sensor verdiene fra uart koblingen. Vi deler så buffer verdiene inn i data objektet og deler det i de forskjellige pakkene.

def handlePackets(accPacketData, angVelPacketData, angPacketData):
    ax, ay, az = handleAccPacket(accPacketData)
    wx, wy, wz = handleAngVelPacket(angVelPacketData)
    roll, pitch, yaw = handleAngPacket(angPacketData)
    return ax, ay, az, wx, wy, wz, roll, pitch, yaw

Her kaller vi de individuelle prosedyrene for de forskjellige pakkene.

def handleAccPacket(accPacketData):
    if len(accPacketData) == PACKET_LENGTH and accPacketData[BYTE_PACKET_HEADER] is PACKET_HEADER and accPacketData[BYTE_PACKET_TYPE] is ACC_PACKET_HEADER:
        ax = (CALCULATION_GRAVITY_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(accPacketData, BYTE_AXH_DATA_CONTENT, BYTE_AXL_DATA_CONTENT)
        ay = (CALCULATION_GRAVITY_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(accPacketData, BYTE_AYH_DATA_CONTENT, BYTE_AYL_DATA_CONTENT)
        az = (CALCULATION_GRAVITY_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(accPacketData, BYTE_AZH_DATA_CONTENT, BYTE_AZL_DATA_CONTENT)
        return ax, ay, az
    else:
        return UNDEFINED, UNDEFINED ,UNDEFINED

Her beregner vi aksellerasjonene i de forskjellige aksene. Formulene for dette finner du lenger oppe.

def handleAngVelPacket(angVelPacketData):
    if len(angVelPacketData) == PACKET_LENGTH and angVelPacketData[BYTE_PACKET_HEADER] is PACKET_HEADER and angVelPacketData[BYTE_PACKET_TYPE] is ANGVEL_PACKET_HEADER:
        wx = (CALCULATION_ROTATION_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(angVelPacketData, BYTE_WXH_DATA_CONTENT, BYTE_WXL_DATA_CONTENT)
        wy = (CALCULATION_ROTATION_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(angVelPacketData, BYTE_WYH_DATA_CONTENT, BYTE_WYL_DATA_CONTENT)
        wz = (CALCULATION_ROTATION_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(angVelPacketData, BYTE_WZH_DATA_CONTENT, BYTE_WZL_DATA_CONTENT)
        return wx, wy, wz
    else:
        return UNDEFINED, UNDEFINED , UNDEFINED

Her beregner vi rotasjonshastighet.

def handleAngPacket(angPacketData):
    if len(angPacketData) == PACKET_LENGTH and angPacketData[BYTE_PACKET_HEADER] is PACKET_HEADER and angPacketData[BYTE_PACKET_TYPE] is ANG_PACKET:
        roll = (CALCULATION_ANGLE_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(angPacketData, BYTE_ROLLH_DATA_CONTENT, BYTE_ROLLL_DATA_CONTENT)
        pitch = (CALCULATION_ANGLE_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(angPacketData, BYTE_PITCHH_DATA_CONTENT, BYTE_PITCHL_DATA_CONTENT)
        yaw = (CALCULATION_ANGLE_CONSTANT / MAX_16_BIT_INTEGER_SIZE) * convertShort(angPacketData, BYTE_YAWH_DATA_CONTENT, BYTE_YAWL_DATA_CONTENT)
        return roll, pitch, yaw
    return UNDEFINED, UNDEFINED, UNDEFINED

Beregner vinkel i de forskjellige aksene.

def convertShort(packet, highByte, lowByte):
    b = bytearray(2)
    b[0] = packet[highByte]
    b[1] = packet[lowByte]
    result = ustruct.unpack(">h", b)[0]
    return result

Dette er den viktige delen for å konvertere to byte til en short. En short er besår av to bytes.