mirror of https://github.com/periph/devices
Working firmata and onewire
imle
5 years ago
parent
b5024d6816
commit
12356d3840
@ -0,0 +1,27 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"fmt"
|
||||
)
|
||||
|
||||
type AnalogMappingResponse struct {
|
||||
AnalogPinToDigital []uint8
|
||||
DigitalPinToAnalog map[uint8]uint8
|
||||
}
|
||||
|
||||
func (a AnalogMappingResponse) String() string {
|
||||
str := bytes.Buffer{}
|
||||
for analogPin, digitalPin := range a.AnalogPinToDigital {
|
||||
_, _ = fmt.Fprintf(&str, "A%d: %d\n", analogPin, digitalPin)
|
||||
}
|
||||
return str.String()
|
||||
}
|
||||
|
||||
type ExtendedAnalogMappingResponse struct {
|
||||
Pin uint8
|
||||
}
|
||||
|
||||
func (a ExtendedAnalogMappingResponse) String() string {
|
||||
return fmt.Sprintf("%d", a.Pin)
|
||||
}
|
||||
@ -0,0 +1,51 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"fmt"
|
||||
|
||||
"periph.io/x/conn/v3/pin"
|
||||
)
|
||||
|
||||
var pinModeOrder = []pin.Func{
|
||||
PinFuncDigitalInput,
|
||||
PinFuncDigitalOutput,
|
||||
PinFuncAnalogInput,
|
||||
PinFuncPWM,
|
||||
PinFuncServo,
|
||||
PinFuncShift,
|
||||
PinFuncI2C,
|
||||
PinFuncOneWire,
|
||||
PinFuncStepper,
|
||||
PinFuncEncoder,
|
||||
PinFuncSerial,
|
||||
PinFuncInputPullUp,
|
||||
PinFuncSPI,
|
||||
PinFuncSonar,
|
||||
PinFuncTone,
|
||||
PinFuncDHT,
|
||||
}
|
||||
|
||||
const CapabilityResponsePinDelimiter = 0x7F
|
||||
|
||||
type CapabilityResponse struct {
|
||||
PinToModeToResolution []map[pin.Func]uint8
|
||||
SupportedPinModes [][]pin.Func
|
||||
}
|
||||
|
||||
func (c CapabilityResponse) String() string {
|
||||
str := bytes.Buffer{}
|
||||
for p, modeMap := range c.PinToModeToResolution {
|
||||
_, _ = fmt.Fprintf(&str, "pin %2v: [", p)
|
||||
if len(modeMap) > 0 {
|
||||
for _, mode := range pinModeOrder {
|
||||
if resolution, ok := modeMap[mode]; ok {
|
||||
_, _ = fmt.Fprintf(&str, "%s: %d, ", mode, resolution)
|
||||
}
|
||||
}
|
||||
str.Truncate(str.Len() - 2)
|
||||
}
|
||||
_, _ = fmt.Fprintf(&str, "]\n")
|
||||
}
|
||||
return str.String()
|
||||
}
|
||||
@ -0,0 +1,697 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"bufio"
|
||||
"errors"
|
||||
"fmt"
|
||||
"io"
|
||||
"sync"
|
||||
|
||||
"periph.io/x/conn/v3/gpio"
|
||||
"periph.io/x/conn/v3/onewire"
|
||||
"periph.io/x/conn/v3/pin"
|
||||
)
|
||||
|
||||
// These max values are for data bytes as, within firmata, data is 7 bits long.
|
||||
const (
|
||||
MaxUInt8 uint8 = (1<<8 - 1) >> 1
|
||||
MaxUInt16 uint16 = (1<<16 - 1) >> 2
|
||||
)
|
||||
|
||||
var commandResponseMap = map[SysExCmd]SysExCmd{
|
||||
SysExAnalogMappingQuery: SysExAnalogMappingResponse,
|
||||
SysExCapabilityQuery: SysExCapabilityResponse,
|
||||
SysExPinStateQuery: SysExPinStateResponse,
|
||||
}
|
||||
|
||||
type ClientI interface {
|
||||
SendSysEx(SysExCmd, ...byte) (chan []byte, error)
|
||||
SendReset() error
|
||||
ExtendedReportAnalogPin(uint8, int) error
|
||||
CapabilityQuery() (chan CapabilityResponse, error)
|
||||
PinStateQuery(uint8) (chan PinStateResponse, error)
|
||||
ReportFirmware() (chan FirmwareReport, error)
|
||||
SetPinMode(uint8, pin.Func) error
|
||||
SetAnalogPinReporting(uint8, bool) error
|
||||
SetDigitalPinReporting(uint8, bool) error
|
||||
SetDigitalPortReporting(uint8, bool) error
|
||||
SetSamplingInterval(uint16) error
|
||||
SetDigitalPinValue(p uint8, value gpio.Level) error
|
||||
SendAnalogMappingQuery() (chan AnalogMappingResponse, error)
|
||||
SetI2CMessageChannel(address uint8, ch chan I2CPacket)
|
||||
WriteI2CData(address uint8, restart bool, data []uint8) error
|
||||
ReadI2CData(address uint8, restart bool, len uint16) error
|
||||
ReadI2CRegister(address uint8, restart bool, register uint8, len uint16) error
|
||||
SendI2CConfig(delayMicroseconds uint8) error
|
||||
AnalogPinToDigitalPin(p uint8) (uint8, error)
|
||||
SetAnalogIOMessageListener(p uint8, ch chan uint16) (release func(), err error)
|
||||
SetDigitalIOMessageListener(p uint8, ch chan gpio.Level) (release func(), err error)
|
||||
SendAnalogIOMessage(uint8, uint16) error
|
||||
|
||||
OpenOneWireBus(p uint8) (bus onewire.BusCloser, err error)
|
||||
SetOneWireListener(uint8, chan []byte) (release func(), err error)
|
||||
|
||||
GetPinName(uint8) string
|
||||
GetPinFunctions(uint8) []pin.Func
|
||||
|
||||
Close() error
|
||||
}
|
||||
|
||||
type Client struct {
|
||||
board io.ReadWriteCloser
|
||||
responseChannels map[SysExCmd][]chan []byte
|
||||
sysExListenerChannels map[SysExCmd]chan []byte
|
||||
i2cListeners map[uint8]chan I2CPacket
|
||||
|
||||
onewireListeners map[uint8]chan []byte
|
||||
onewireMU sync.Mutex
|
||||
|
||||
digitalIOMessageChannels map[uint8]chan gpio.Level
|
||||
digitalPinMU sync.Mutex
|
||||
analogIOMessageChannels map[uint8]chan uint16
|
||||
analogPinMU sync.Mutex
|
||||
|
||||
mu sync.Mutex
|
||||
started bool
|
||||
i2cStarted bool
|
||||
|
||||
// We want to report these to the requester, but also save them for internal use.
|
||||
cr CapabilityResponse
|
||||
amr AnalogMappingResponse
|
||||
}
|
||||
|
||||
func NewClient(board io.ReadWriteCloser) *Client {
|
||||
return &Client{
|
||||
board: board,
|
||||
responseChannels: map[SysExCmd][]chan []byte{},
|
||||
sysExListenerChannels: map[SysExCmd]chan []byte{},
|
||||
i2cListeners: map[uint8]chan I2CPacket{},
|
||||
|
||||
onewireListeners: map[uint8]chan []byte{},
|
||||
|
||||
digitalIOMessageChannels: map[uint8]chan gpio.Level{},
|
||||
analogIOMessageChannels: map[uint8]chan uint16{},
|
||||
}
|
||||
}
|
||||
|
||||
func (c *Client) Close() error {
|
||||
c.mu.Lock()
|
||||
defer c.mu.Unlock()
|
||||
|
||||
c.started = false
|
||||
return c.board.Close()
|
||||
}
|
||||
|
||||
type flusher interface {
|
||||
Flush()
|
||||
}
|
||||
|
||||
type flusherErr interface {
|
||||
Flush() error
|
||||
}
|
||||
|
||||
func (c *Client) Start() error {
|
||||
c.mu.Lock()
|
||||
if c.started {
|
||||
c.mu.Unlock()
|
||||
return ErrAlreadyStarted
|
||||
}
|
||||
c.started = true
|
||||
c.mu.Unlock()
|
||||
|
||||
if b, ok := c.board.(flusher); ok {
|
||||
b.Flush()
|
||||
} else if b, ok := c.board.(flusherErr); ok {
|
||||
if err := b.Flush(); err != nil {
|
||||
return err
|
||||
}
|
||||
}
|
||||
|
||||
firmChannel := make(chan []byte, 1)
|
||||
// Don't call ReportFirmware as it is automatic, but we want to register a listener for it.
|
||||
c.mu.Lock()
|
||||
c.responseChannels[SysExReportFirmware] = []chan []byte{firmChannel}
|
||||
c.mu.Unlock()
|
||||
report := c.parseReportFirmware(firmChannel)
|
||||
|
||||
go func() {
|
||||
err := c.responseWatcher()
|
||||
if err != nil {
|
||||
panic(err)
|
||||
}
|
||||
}()
|
||||
|
||||
fmt.Println("Firmware Info:", <-report)
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (c *Client) write(payload []byte, withinMutex func()) error {
|
||||
// Cannot allow multiple writes at the same time.
|
||||
c.mu.Lock()
|
||||
defer c.mu.Unlock()
|
||||
|
||||
//fmt.Println(SprintHexArray(payload))
|
||||
|
||||
// Write to the board.
|
||||
_, err := c.board.Write(payload)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if withinMutex != nil {
|
||||
withinMutex()
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (c *Client) responseWatcher() (err error) {
|
||||
defer func() {
|
||||
if errors.Is(err, io.EOF) {
|
||||
err = ErrDeviceDisconnected
|
||||
}
|
||||
}()
|
||||
|
||||
reader := bufio.NewReader(c.board)
|
||||
for {
|
||||
var data []byte
|
||||
b0, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
mt := MessageType(b0)
|
||||
switch {
|
||||
case mt == ProtocolVersion:
|
||||
var version [2]byte
|
||||
_, err := reader.Read(version[:])
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
fmt.Printf("Protocol Version: 0x%0.2X 0x%0.2X\n", version[0], version[1])
|
||||
case AnalogIOMessage <= mt && mt <= (AnalogIOMessage+0xF):
|
||||
v1, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
v2, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
c.analogIOMessageChannels[b0&0xF] <- TwoByteToWord(v1, v2)
|
||||
case DigitalIOMessage <= mt && mt <= (DigitalIOMessage+0xF):
|
||||
v1, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
v2, err := reader.ReadByte()
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
values := TwoByteToByte(v1, v2)
|
||||
|
||||
port := b0 & 0xF
|
||||
pinMin := port * 8
|
||||
pinMax := (port+1)*8 - 1
|
||||
for p := pinMin; p <= pinMax; p++ {
|
||||
if ch, ok := c.digitalIOMessageChannels[p]; ok {
|
||||
lvl := gpio.Low
|
||||
if values>>p%8 > 0 {
|
||||
lvl = gpio.High
|
||||
}
|
||||
ch <- lvl
|
||||
}
|
||||
}
|
||||
case mt == StartSysEx:
|
||||
data, err = reader.ReadBytes(byte(EndSysEx))
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if len(data) == 0 {
|
||||
return ErrNoDataRead
|
||||
}
|
||||
|
||||
cmd := SysExCmd(data[0])
|
||||
data = data[1 : len(data)-1]
|
||||
|
||||
switch {
|
||||
case cmd == SysExSerialDataV1:
|
||||
fallthrough
|
||||
case cmd == SysExSerialDataV2:
|
||||
return fmt.Errorf("%w: %s", ErrUnsupportedFeature, cmd)
|
||||
case cmd == SysExOneWireData:
|
||||
p := data[1]
|
||||
|
||||
if l, ok := c.onewireListeners[p]; ok {
|
||||
l <- data
|
||||
} else {
|
||||
return fmt.Errorf("%w: onewire cmd:0x%02X pin 0x%02X", ErrUnhandledMessage, data[0], p)
|
||||
}
|
||||
case cmd == SysExI2CReply:
|
||||
address := TwoByteToByte(data[0], data[1])
|
||||
register := TwoByteToByte(data[2], data[3])
|
||||
ch, ok := c.i2cListeners[address]
|
||||
if !ok {
|
||||
return fmt.Errorf("%w: 0x%02X", ErrNoI2CListenerForAddress, address)
|
||||
}
|
||||
|
||||
ch <- I2CPacket{
|
||||
Register: register,
|
||||
Data: TwoByteRepresentationToByteSlice(data[4:]),
|
||||
}
|
||||
case c.sysExListenerChannels[cmd] != nil:
|
||||
c.sysExListenerChannels[cmd] <- data
|
||||
case len(c.responseChannels[cmd]) != 0:
|
||||
c.mu.Lock()
|
||||
resp := c.responseChannels[cmd][0]
|
||||
c.responseChannels[cmd] = c.responseChannels[cmd]
|
||||
c.mu.Unlock()
|
||||
|
||||
resp <- data
|
||||
close(resp)
|
||||
case cmd == SysExStringData:
|
||||
fmt.Printf("device: [%s]\n", TwoByteString(data))
|
||||
default:
|
||||
str := ""
|
||||
if cmd == SysExStringData {
|
||||
str = TwoByteString(data)
|
||||
} else {
|
||||
for _, b := range data {
|
||||
str += fmt.Sprintf("%d", b)
|
||||
}
|
||||
}
|
||||
|
||||
return fmt.Errorf("%w: 0x%0.2X: %s", ErrUnexpectedSysExMessageTypeReceived, byte(cmd), str)
|
||||
}
|
||||
default:
|
||||
return fmt.Errorf("%w: 0x%0.2X", ErrInvalidMessageTypeStart, b0)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (c *Client) SendReset() error {
|
||||
return c.write([]byte{byte(SystemReset)}, nil)
|
||||
}
|
||||
|
||||
func (c *Client) AnalogPinToDigitalPin(p uint8) (uint8, error) {
|
||||
if int(p) > len(c.amr.AnalogPinToDigital) {
|
||||
return 0, ErrInvalidAnalogPin
|
||||
}
|
||||
|
||||
return c.amr.AnalogPinToDigital[p], nil
|
||||
}
|
||||
|
||||
func (c *Client) SetPinMode(p uint8, mode pin.Func) error {
|
||||
return c.write([]uint8{uint8(SetPinMode), p, pinFuncToModeMap[mode]}, nil)
|
||||
}
|
||||
|
||||
func (c *Client) SetDigitalPinValue(p uint8, value gpio.Level) error {
|
||||
v := byte(0)
|
||||
if value {
|
||||
v = 1
|
||||
}
|
||||
return c.write([]uint8{uint8(SetDigitalPinValue), p, v}, nil)
|
||||
}
|
||||
|
||||
func (c *Client) SendSysEx(cmd SysExCmd, payload ...byte) (chan []byte, error) {
|
||||
// Create a response channel.
|
||||
var data chan []byte
|
||||
|
||||
err := c.write(append([]byte{byte(StartSysEx), byte(cmd)}, append(payload, byte(EndSysEx))...), func() {
|
||||
// This assumes that SysEx commands of the same type are responded to in order.
|
||||
if resp, ok := commandResponseMap[cmd]; ok {
|
||||
data = make(chan []byte, 1)
|
||||
c.responseChannels[resp] = append(c.responseChannels[resp], data)
|
||||
}
|
||||
})
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
return data, nil
|
||||
}
|
||||
|
||||
func (c *Client) CapabilityQuery() (chan CapabilityResponse, error) {
|
||||
future, err := c.SendSysEx(SysExCapabilityQuery)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
resp := c.parseCapabilityCommand(future)
|
||||
|
||||
return resp, nil
|
||||
}
|
||||
|
||||
func (c *Client) parseCapabilityCommand(future chan []byte) chan CapabilityResponse {
|
||||
resp := make(chan CapabilityResponse, 1)
|
||||
|
||||
go func() {
|
||||
data := <-future
|
||||
var response = CapabilityResponse{
|
||||
PinToModeToResolution: []map[pin.Func]uint8{{}},
|
||||
SupportedPinModes: [][]pin.Func{{}},
|
||||
}
|
||||
var pindex = 0
|
||||
for i := 0; i < len(data); {
|
||||
if data[i] == CapabilityResponsePinDelimiter {
|
||||
response.PinToModeToResolution = append(response.PinToModeToResolution, map[pin.Func]uint8{})
|
||||
response.SupportedPinModes = append(response.SupportedPinModes, []pin.Func{})
|
||||
i += 1
|
||||
pindex++
|
||||
} else {
|
||||
pinFunc := pinModeToFuncMap[data[i]]
|
||||
response.PinToModeToResolution[pindex][pinFunc] = data[i+1]
|
||||
response.SupportedPinModes[pindex] = append(response.SupportedPinModes[pindex], pinFunc)
|
||||
i += 2
|
||||
}
|
||||
}
|
||||
|
||||
c.cr = response
|
||||
resp <- response
|
||||
close(resp)
|
||||
}()
|
||||
|
||||
return resp
|
||||
}
|
||||
|
||||
func (c *Client) SendAnalogMappingQuery() (chan AnalogMappingResponse, error) {
|
||||
future, err := c.SendSysEx(SysExAnalogMappingQuery)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
resp := c.parseAnalogMappingQuery(future)
|
||||
|
||||
return resp, nil
|
||||
}
|
||||
|
||||
func (c *Client) parseAnalogMappingQuery(future chan []byte) chan AnalogMappingResponse {
|
||||
resp := make(chan AnalogMappingResponse, 1)
|
||||
|
||||
go func() {
|
||||
data := <-future
|
||||
var response = AnalogMappingResponse{
|
||||
AnalogPinToDigital: []uint8{},
|
||||
DigitalPinToAnalog: map[uint8]uint8{},
|
||||
}
|
||||
for i := 0; i < len(data); i++ {
|
||||
if data[i] != CapabilityResponsePinDelimiter {
|
||||
response.DigitalPinToAnalog[uint8(i)] = uint8(len(response.AnalogPinToDigital))
|
||||
response.AnalogPinToDigital = append(response.AnalogPinToDigital, uint8(i))
|
||||
}
|
||||
}
|
||||
|
||||
c.amr = response
|
||||
resp <- response
|
||||
close(resp)
|
||||
}()
|
||||
|
||||
return resp
|
||||
}
|
||||
|
||||
func (c *Client) ExtendedReportAnalogPin(p uint8, value int) error {
|
||||
if value > 0xFFFFFFFFFFFFFF {
|
||||
return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange)
|
||||
}
|
||||
|
||||
_, err := c.SendSysEx(SysExExtendedAnalog, p, uint8(value), uint8(value>>7), uint8(value>>14))
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (c *Client) PinStateQuery(p uint8) (chan PinStateResponse, error) {
|
||||
future, err := c.SendSysEx(SysExPinStateQuery, p)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
resp := c.parsePinStateQuery(future)
|
||||
|
||||
return resp, nil
|
||||
}
|
||||
|
||||
func (c *Client) parsePinStateQuery(future chan []byte) chan PinStateResponse {
|
||||
resp := make(chan PinStateResponse, 1)
|
||||
|
||||
go func() {
|
||||
data := <-future
|
||||
var ps = PinStateResponse{
|
||||
Pin: data[0],
|
||||
Mode: pinModeToFuncMap[data[1]],
|
||||
State: 0,
|
||||
}
|
||||
|
||||
for i, b := range data[2:] {
|
||||
ps.State |= int(b << (i * 7))
|
||||
}
|
||||
|
||||
resp <- ps
|
||||
close(resp)
|
||||
}()
|
||||
|
||||
return resp
|
||||
}
|
||||
|
||||
func (c *Client) ReportFirmware() (chan FirmwareReport, error) {
|
||||
future, err := c.SendSysEx(SysExReportFirmware)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
resp := c.parseReportFirmware(future)
|
||||
|
||||
return resp, nil
|
||||
}
|
||||
|
||||
func (c *Client) parseReportFirmware(future chan []byte) chan FirmwareReport {
|
||||
resp := make(chan FirmwareReport, 1)
|
||||
|
||||
go func() {
|
||||
data := <-future
|
||||
var rc = FirmwareReport{
|
||||
Major: data[0],
|
||||
Minor: data[1],
|
||||
Name: data[2:],
|
||||
}
|
||||
|
||||
resp <- rc
|
||||
close(resp)
|
||||
}()
|
||||
|
||||
return resp
|
||||
}
|
||||
|
||||
func (c *Client) SetAnalogPinReporting(analogPin uint8, report bool) error {
|
||||
v := byte(0)
|
||||
if report {
|
||||
v = 1
|
||||
}
|
||||
|
||||
return c.write([]byte{byte(ReportAnalogPin) | (analogPin & 0xF), v}, nil)
|
||||
}
|
||||
|
||||
func (c *Client) SetDigitalPinReporting(p uint8, report bool) error {
|
||||
return c.SetDigitalPortReporting(p%8, report)
|
||||
}
|
||||
|
||||
func (c *Client) SetDigitalPortReporting(port uint8, report bool) error {
|
||||
v := byte(0)
|
||||
if report {
|
||||
v = 1
|
||||
}
|
||||
|
||||
return c.write([]byte{byte(ReportDigitalPort) | (port & 0xF), v}, nil)
|
||||
}
|
||||
|
||||
func (c *Client) SetSamplingInterval(ms uint16) error {
|
||||
if ms > MaxUInt16 {
|
||||
return fmt.Errorf("%w: 0x0 - 0x%X", ErrValueOutOfRange, MaxUInt16)
|
||||
}
|
||||
return c.write([]byte{byte(SysExSamplingInterval), byte(ms), byte(ms >> 7)}, nil)
|
||||
}
|
||||
|
||||
// This function only supports 7-bit I2C addresses
|
||||
func (c *Client) WriteI2CData(address uint8, restart bool, data []uint8) error {
|
||||
if !c.i2cStarted {
|
||||
return ErrI2CNotEnabled
|
||||
}
|
||||
|
||||
byte2 := byte(I2CModeWrite)
|
||||
if restart {
|
||||
byte2 &= I2CRestartTransmission
|
||||
}
|
||||
|
||||
payload := append([]byte{address, byte2}, ByteSliceToTwoByteRepresentation(data)...)
|
||||
_, err := c.SendSysEx(SysExI2CRequest, payload...)
|
||||
return err
|
||||
}
|
||||
|
||||
// This function only supports 7-bit I2C addresses
|
||||
func (c *Client) ReadI2CData(address uint8, restart bool, length uint16) error {
|
||||
if !c.i2cStarted {
|
||||
return ErrI2CNotEnabled
|
||||
}
|
||||
|
||||
if length > MaxUInt16 {
|
||||
return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange)
|
||||
}
|
||||
|
||||
byte2 := byte(I2CModeRead)
|
||||
if restart {
|
||||
byte2 &= I2CRestartTransmission
|
||||
}
|
||||
|
||||
lLSB, lMSB := WordToTwoByte(length)
|
||||
|
||||
_, err := c.SendSysEx(SysExI2CRequest, address, byte2, lLSB, lMSB)
|
||||
return err
|
||||
}
|
||||
|
||||
// This function only supports 7-bit I2C addresses
|
||||
func (c *Client) ReadI2CRegister(address uint8, restart bool, register uint8, length uint16) error {
|
||||
if !c.i2cStarted {
|
||||
return ErrI2CNotEnabled
|
||||
}
|
||||
|
||||
if length > MaxUInt16 {
|
||||
return fmt.Errorf("%w: 0x0 - 0xFFFFFFFFFFFFFF", ErrValueOutOfRange)
|
||||
}
|
||||
|
||||
byte2 := byte(I2CModeRead)
|
||||
if restart {
|
||||
byte2 &= I2CRestartTransmission
|
||||
}
|
||||
|
||||
rLSB, rMSB := ByteToTwoByte(register)
|
||||
lLSB, lMSB := WordToTwoByte(length)
|
||||
|
||||
_, err := c.SendSysEx(SysExI2CRequest, address, byte2, rLSB, rMSB, lLSB, lMSB)
|
||||
return err
|
||||
}
|
||||
|
||||
func (c *Client) SendI2CConfig(delayMicroseconds uint8) error {
|
||||
micLSB, micMSB := ByteToTwoByte(delayMicroseconds)
|
||||
_, err := c.SendSysEx(SysExI2CConfig, micLSB, micMSB)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
c.i2cStarted = true
|
||||
return nil
|
||||
}
|
||||
|
||||
// This function only supports 7-bit I2C addresses
|
||||
func (c *Client) SetI2CMessageChannel(address uint8, ch chan I2CPacket) {
|
||||
if c.i2cListeners[address] != nil {
|
||||
close(c.i2cListeners[address])
|
||||
}
|
||||
|
||||
c.i2cListeners[address] = ch
|
||||
}
|
||||
|
||||
func (c *Client) releaseAnalogIOMessageListener(p uint8) {
|
||||
c.analogPinMU.Lock()
|
||||
defer c.analogPinMU.Unlock()
|
||||
|
||||
delete(c.analogIOMessageChannels, p)
|
||||
}
|
||||
|
||||
func (c *Client) SetAnalogIOMessageListener(p uint8, ch chan uint16) (release func(), err error) {
|
||||
c.analogPinMU.Lock()
|
||||
defer c.analogPinMU.Unlock()
|
||||
|
||||
if c.analogIOMessageChannels[p] != nil {
|
||||
return nil, ErrPinListenerNotReleased
|
||||
}
|
||||
|
||||
c.analogIOMessageChannels[p] = ch
|
||||
|
||||
return func() { c.releaseAnalogIOMessageListener(p) }, nil
|
||||
}
|
||||
|
||||
func (c *Client) releaseDigitalIOMessageListener(p uint8) {
|
||||
c.digitalPinMU.Lock()
|
||||
defer c.digitalPinMU.Unlock()
|
||||
|
||||
delete(c.digitalIOMessageChannels, p)
|
||||
}
|
||||
|
||||
func (c *Client) SetDigitalIOMessageListener(p uint8, ch chan gpio.Level) (release func(), err error) {
|
||||
c.digitalPinMU.Lock()
|
||||
defer c.digitalPinMU.Unlock()
|
||||
|
||||
if c.digitalIOMessageChannels[p] != nil {
|
||||
return nil, ErrPinListenerNotReleased
|
||||
}
|
||||
|
||||
c.digitalIOMessageChannels[p] = ch
|
||||
|
||||
return func() { c.releaseDigitalIOMessageListener(p) }, nil
|
||||
}
|
||||
|
||||
func (c *Client) releaseOneWireListener(p uint8) {
|
||||
c.onewireMU.Lock()
|
||||
defer c.onewireMU.Unlock()
|
||||
|
||||
delete(c.onewireListeners, p)
|
||||
}
|
||||
|
||||
func (c *Client) SetOneWireListener(p uint8, ch chan []byte) (release func(), err error) {
|
||||
c.onewireMU.Lock()
|
||||
defer c.onewireMU.Unlock()
|
||||
|
||||
if c.onewireListeners[p] != nil {
|
||||
return nil, ErrPinListenerNotReleased
|
||||
}
|
||||
|
||||
c.onewireListeners[p] = ch
|
||||
|
||||
return func() { c.releaseOneWireListener(p) }, nil
|
||||
}
|
||||
|
||||
func (c *Client) OpenOneWireBus(p uint8) (bus onewire.BusCloser, err error) {
|
||||
c.onewireMU.Lock()
|
||||
defer c.onewireMU.Unlock()
|
||||
|
||||
if c.onewireListeners[p] != nil {
|
||||
return nil, ErrPinListenerNotReleased
|
||||
}
|
||||
|
||||
// Need to run configure or firmata will not initialize.
|
||||
if _, err := c.SendSysEx(SysExOneWireData, byte(OneWireInstructionConfigure), p, 0x00); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
return newOneWireBus(c, newPin(c, p), func() error {
|
||||
c.releaseOneWireListener(p)
|
||||
return nil
|
||||
}), nil
|
||||
}
|
||||
|
||||
func (c *Client) SendAnalogIOMessage(p uint8, value uint16) error {
|
||||
if p > 0xF {
|
||||
return ErrAnalogIOMessagePinOutOfRange
|
||||
}
|
||||
|
||||
lsb, msb := WordToTwoByte(value)
|
||||
|
||||
return c.write([]byte{byte(AnalogIOMessage) | p, lsb, msb}, nil)
|
||||
}
|
||||
|
||||
func (c *Client) GetPinName(p uint8) string {
|
||||
if v, ok := c.amr.DigitalPinToAnalog[p]; ok {
|
||||
return fmt.Sprintf("A%d", v)
|
||||
}
|
||||
return fmt.Sprintf("%d", p)
|
||||
}
|
||||
|
||||
func (c *Client) GetPinFunctions(p uint8) []pin.Func {
|
||||
return c.cr.SupportedPinModes[int(p)]
|
||||
}
|
||||
@ -0,0 +1,24 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"errors"
|
||||
)
|
||||
|
||||
var (
|
||||
ErrDeviceDisconnected = errors.New("device disconnected")
|
||||
ErrUnsupportedFeature = errors.New("unsupported feature")
|
||||
ErrUnhandledMessage = errors.New("message did not have an active listener")
|
||||
ErrInvalidMessageTypeStart = errors.New("invalid message type start")
|
||||
ErrNoDataRead = errors.New("no data read")
|
||||
ErrUnexpectedSysExMessageTypeReceived = errors.New("unexpected sysex message type")
|
||||
ErrAlreadyStarted = errors.New("client already started")
|
||||
ErrValueOutOfRange = errors.New("value is out of range")
|
||||
ErrNoI2CListenerForAddress = errors.New("no i2c listener registered for address")
|
||||
ErrInvalidFirmataI2CBus = errors.New("firmata does not support multiple i2c buses")
|
||||
ErrInvalidAnalogPin = errors.New("analog pin is outside of range")
|
||||
ErrI2CNotEnabled = errors.New("i2c must started to use")
|
||||
ErrInvalidOneWirePin = errors.New("onewire pin number cannot exceed 0x7F")
|
||||
ErrAnalogIOMessagePinOutOfRange = errors.New("analog io message pin number cannot exceed 0xF")
|
||||
ErrDigitalIOMessagePinOutOfRange = errors.New("digital io message port number cannot exceed 0xF")
|
||||
ErrPinListenerNotReleased = errors.New("pin listener is already set for pin")
|
||||
)
|
||||
@ -0,0 +1,7 @@
|
||||
package firmata
|
||||
|
||||
type Feature struct {
|
||||
ID uint8
|
||||
VersionMajor uint8
|
||||
VersionMinor uint8
|
||||
}
|
||||
@ -0,0 +1,216 @@
|
||||
package firmatareg
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"strconv"
|
||||
"strings"
|
||||
"sync"
|
||||
|
||||
"periph.io/x/devices/v3/firmata"
|
||||
)
|
||||
|
||||
// Opener opens a handle to a firmata device.
|
||||
//
|
||||
// It is provided by the actual device driver.
|
||||
type Opener func() (firmata.ClientI, error)
|
||||
|
||||
// Ref references a Firmata device.
|
||||
//
|
||||
// It is returned by All() to enumerate all registered devices.
|
||||
type Ref struct {
|
||||
// Name of the device.
|
||||
//
|
||||
// It must be unique across the host.
|
||||
Name string
|
||||
// Aliases are the alternative names that can be used to reference this bus.
|
||||
Aliases []string
|
||||
// Open is the factory to open a handle to this Firmata device.
|
||||
Open Opener
|
||||
}
|
||||
|
||||
// Open opens a firmata device by its name, an alias or its file path and returns
|
||||
// a handle to it.
|
||||
//
|
||||
// Specify the empty string "" to get the first available device. This is the
|
||||
// recommended default value unless an application knows the exact device to use.
|
||||
//
|
||||
// Each device can register multiple aliases, each leading to the same device handle.
|
||||
//
|
||||
// "file path" is a generic concept that is highly dependent on the platform
|
||||
// and OS. Depending on the OS, a serial port can be `/dev/tty*` or `COM*`.
|
||||
func Open(name string) (firmata.ClientI, error) {
|
||||
var r *Ref
|
||||
var err error
|
||||
func() {
|
||||
mu.Lock()
|
||||
defer mu.Unlock()
|
||||
if len(byName) == 0 {
|
||||
err = errors.New("firmatareg: no device found; did you forget to call Init()")
|
||||
return
|
||||
}
|
||||
if len(name) == 0 {
|
||||
r = getDefault()
|
||||
return
|
||||
}
|
||||
// Try by name, by alias, by number.
|
||||
if r = byName[name]; r == nil {
|
||||
r = byAlias[name]
|
||||
}
|
||||
}()
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
if r == nil {
|
||||
return nil, errors.New("firmatareg: can't open unknown device: " + strconv.Quote(name))
|
||||
}
|
||||
return r.Open()
|
||||
}
|
||||
|
||||
// All returns a copy of all the registered references to all know firmata devices
|
||||
// available on this host.
|
||||
//
|
||||
// The list is sorted by the bus name.
|
||||
func All() []*Ref {
|
||||
mu.Lock()
|
||||
defer mu.Unlock()
|
||||
out := make([]*Ref, 0, len(byName))
|
||||
for _, v := range byName {
|
||||
r := &Ref{Name: v.Name, Aliases: make([]string, len(v.Aliases)), Open: v.Open}
|
||||
copy(r.Aliases, v.Aliases)
|
||||
out = insertRef(out, r)
|
||||
}
|
||||
return out
|
||||
}
|
||||
|
||||
// Register registers a firmata device.
|
||||
//
|
||||
// Registering the same device name twice is an error, e.g. o.Name().
|
||||
func Register(name string, aliases []string, o Opener) error {
|
||||
if len(name) == 0 {
|
||||
return errors.New("firmatareg: can't register a bus with no name")
|
||||
}
|
||||
if o == nil {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with nil Opener")
|
||||
}
|
||||
if _, err := strconv.Atoi(name); err == nil {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with name being only a number")
|
||||
}
|
||||
if strings.Contains(name, ":") {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with name containing ':'")
|
||||
}
|
||||
for _, alias := range aliases {
|
||||
if len(alias) == 0 {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an empty alias")
|
||||
}
|
||||
if name == alias {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias the same as the bus name")
|
||||
}
|
||||
if _, err := strconv.Atoi(alias); err == nil {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias that is a number: " + strconv.Quote(alias))
|
||||
}
|
||||
if strings.Contains(alias, ":") {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " with an alias containing ':': " + strconv.Quote(alias))
|
||||
}
|
||||
}
|
||||
|
||||
mu.Lock()
|
||||
defer mu.Unlock()
|
||||
if _, ok := byName[name]; ok {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice")
|
||||
}
|
||||
if _, ok := byAlias[name]; ok {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; it is already an alias")
|
||||
}
|
||||
for _, alias := range aliases {
|
||||
if _, ok := byName[alias]; ok {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; alias " + strconv.Quote(alias) + " is already a bus")
|
||||
}
|
||||
if _, ok := byAlias[alias]; ok {
|
||||
return errors.New("firmatareg: can't register bus " + strconv.Quote(name) + " twice; alias " + strconv.Quote(alias) + " is already an alias")
|
||||
}
|
||||
}
|
||||
|
||||
r := &Ref{Name: name, Aliases: make([]string, len(aliases)), Open: o}
|
||||
copy(r.Aliases, aliases)
|
||||
byName[name] = r
|
||||
for _, alias := range aliases {
|
||||
byAlias[alias] = r
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
// Unregister removes a previously registered firmata device.
|
||||
//
|
||||
// This can happen when a firmata device is exposed via a USB device and the device
|
||||
// is unplugged.
|
||||
func Unregister(name string) error {
|
||||
mu.Lock()
|
||||
defer mu.Unlock()
|
||||
r := byName[name]
|
||||
if r == nil {
|
||||
return errors.New("firmatareg: can't unregister unknown bus name " + strconv.Quote(name))
|
||||
}
|
||||
delete(byName, name)
|
||||
for _, alias := range r.Aliases {
|
||||
delete(byAlias, alias)
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
//
|
||||
|
||||
var (
|
||||
mu sync.Mutex
|
||||
byName = map[string]*Ref{}
|
||||
// Caches
|
||||
byNumber = map[int]*Ref{}
|
||||
byAlias = map[string]*Ref{}
|
||||
)
|
||||
|
||||
// getDefault returns the Ref that should be used as the default bus.
|
||||
func getDefault() *Ref {
|
||||
var o *Ref
|
||||
if len(byNumber) == 0 {
|
||||
// Fallback to use byName using a lexical sort.
|
||||
name := ""
|
||||
for n, o2 := range byName {
|
||||
if len(name) == 0 || n < name {
|
||||
o = o2
|
||||
name = n
|
||||
}
|
||||
}
|
||||
return o
|
||||
}
|
||||
number := int((^uint(0)) >> 1)
|
||||
for n, o2 := range byNumber {
|
||||
if number > n {
|
||||
number = n
|
||||
o = o2
|
||||
}
|
||||
}
|
||||
return o
|
||||
}
|
||||
|
||||
func insertRef(l []*Ref, r *Ref) []*Ref {
|
||||
n := r.Name
|
||||
i := search(len(l), func(i int) bool { return l[i].Name > n })
|
||||
l = append(l, nil)
|
||||
copy(l[i+1:], l[i:])
|
||||
l[i] = r
|
||||
return l
|
||||
}
|
||||
|
||||
// search implements the same algorithm as sort.Search().
|
||||
//
|
||||
// It was extracted to not depend on sort, which depends on reflect.
|
||||
func search(n int, f func(int) bool) int {
|
||||
lo := 0
|
||||
for hi := n; lo < hi; {
|
||||
if i := int(uint(lo+hi) >> 1); !f(i) {
|
||||
lo = i + 1
|
||||
} else {
|
||||
hi = i
|
||||
}
|
||||
}
|
||||
return lo
|
||||
}
|
||||
@ -0,0 +1,15 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
)
|
||||
|
||||
type FirmwareReport struct {
|
||||
Major byte
|
||||
Minor byte
|
||||
Name []byte
|
||||
}
|
||||
|
||||
func (r FirmwareReport) String() string {
|
||||
return fmt.Sprintf("%s [%d.%d]", TwoByteString(r.Name), r.Major, r.Minor)
|
||||
}
|
||||
@ -0,0 +1,33 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"periph.io/x/conn/v3/gpio"
|
||||
"periph.io/x/conn/v3/physic"
|
||||
)
|
||||
|
||||
type I2CBus struct {
|
||||
}
|
||||
|
||||
func (i *I2CBus) SCL() gpio.PinIO {
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (i *I2CBus) SDA() gpio.PinIO {
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (i *I2CBus) Close() error {
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (i *I2CBus) String() string {
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (i *I2CBus) Tx(addr uint16, w, r []byte) error {
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (i *I2CBus) SetSpeed(f physic.Frequency) error {
|
||||
panic("implement me")
|
||||
}
|
||||
@ -0,0 +1,20 @@
|
||||
package firmata
|
||||
|
||||
const (
|
||||
I2CRestartTransmission uint8 = 0b01000000
|
||||
I2CModeMask uint8 = 0b00011000
|
||||
)
|
||||
|
||||
type I2CMode uint8
|
||||
|
||||
const (
|
||||
I2CModeWrite I2CMode = 0b00000000
|
||||
I2CModeRead I2CMode = 0b00001000
|
||||
I2CModeReadContinuously I2CMode = 0b00010000
|
||||
I2CModeStopReading I2CMode = 0b00011000
|
||||
)
|
||||
|
||||
type I2CPacket struct {
|
||||
Register uint8
|
||||
Data []byte
|
||||
}
|
||||
@ -0,0 +1,50 @@
|
||||
package firmata
|
||||
|
||||
type (
|
||||
MessageType uint8
|
||||
)
|
||||
|
||||
const (
|
||||
AnalogIOMessage MessageType = 0xE0 // pin # LSB(bits 0-6) MSB(bits 7-13)
|
||||
DigitalIOMessage MessageType = 0x90 // port LSB(bits 0-6) MSB(bits 7-13)
|
||||
ReportAnalogPin MessageType = 0xC0 // pin # disable/enable(0/1) - n/a -
|
||||
ReportDigitalPort MessageType = 0xD0 // port disable/enable(0/1) - n/a -
|
||||
StartSysEx MessageType = 0xF0 //
|
||||
SetPinMode MessageType = 0xF4 // pin # (0-127) pin mode
|
||||
SetDigitalPinValue MessageType = 0xF5 // pin # (0-127) pin value(0/1)
|
||||
EndSysEx MessageType = 0xF7 //
|
||||
ProtocolVersion MessageType = 0xF9 // major version minor version
|
||||
SystemReset MessageType = 0xFF //
|
||||
)
|
||||
|
||||
var messageTypeToStringMap = map[MessageType]string{
|
||||
AnalogIOMessage: "AnalogIOMessage",
|
||||
DigitalIOMessage: "DigitalIOMessage",
|
||||
ReportAnalogPin: "ReportAnalogPin",
|
||||
ReportDigitalPort: "ReportDigitalPort",
|
||||
StartSysEx: "StartSysEx",
|
||||
SetPinMode: "SetPinMode",
|
||||
SetDigitalPinValue: "SetDigitalPinValue",
|
||||
EndSysEx: "EndSysEx",
|
||||
ProtocolVersion: "ProtocolVersion",
|
||||
SystemReset: "SystemReset",
|
||||
}
|
||||
|
||||
func (m MessageType) String() string {
|
||||
switch {
|
||||
case AnalogIOMessage <= m && m <= (AnalogIOMessage+0xF):
|
||||
return messageTypeToStringMap[AnalogIOMessage]
|
||||
case DigitalIOMessage <= m && m <= (DigitalIOMessage+0xF):
|
||||
return messageTypeToStringMap[DigitalIOMessage]
|
||||
case ReportAnalogPin <= m && m <= (ReportAnalogPin+0xF):
|
||||
return messageTypeToStringMap[ReportAnalogPin]
|
||||
case ReportDigitalPort <= m && m <= (ReportDigitalPort+0xF):
|
||||
return messageTypeToStringMap[ReportDigitalPort]
|
||||
}
|
||||
|
||||
if v, ok := messageTypeToStringMap[m]; ok {
|
||||
return v
|
||||
}
|
||||
|
||||
return "Unknown"
|
||||
}
|
||||
@ -0,0 +1,181 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"encoding/binary"
|
||||
"fmt"
|
||||
"math"
|
||||
"sync"
|
||||
|
||||
"periph.io/x/conn/v3/gpio"
|
||||
"periph.io/x/conn/v3/onewire"
|
||||
)
|
||||
|
||||
type OneWireBus struct {
|
||||
c ClientI
|
||||
q *Pin
|
||||
mu sync.Mutex
|
||||
closer func() error
|
||||
cid uint16
|
||||
}
|
||||
|
||||
func newOneWireBus(c ClientI, q *Pin, closer func() error) *OneWireBus {
|
||||
return &OneWireBus{
|
||||
c: c,
|
||||
q: q,
|
||||
closer: closer,
|
||||
}
|
||||
}
|
||||
|
||||
func (b *OneWireBus) Close() error {
|
||||
return b.closer()
|
||||
}
|
||||
|
||||
func (b *OneWireBus) String() string {
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (b *OneWireBus) Search(alarmOnly bool) ([]onewire.Address, error) {
|
||||
b.mu.Lock()
|
||||
defer b.mu.Unlock()
|
||||
|
||||
ch := make(chan []byte)
|
||||
defer close(ch)
|
||||
|
||||
release, err := b.c.SetOneWireListener(b.q.pin, ch)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
defer release()
|
||||
|
||||
if alarmOnly {
|
||||
if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearchAlarmed), b.q.pin); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
} else {
|
||||
if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearch), b.q.pin); err != nil {
|
||||
return nil, err
|
||||
}
|
||||
}
|
||||
|
||||
data := <-ch
|
||||
|
||||
ins := OneWireInstruction(data[0])
|
||||
pin := data[1]
|
||||
|
||||
if ins != OneWireInstructionSearchAlarmedReply && ins != OneWireInstructionSearchReply {
|
||||
return nil, fmt.Errorf("%w: did not receive search reply", ErrUnhandledMessage)
|
||||
}
|
||||
if pin != b.q.pin {
|
||||
return nil, fmt.Errorf("%w: received message from wrong bus", ErrInvalidOneWirePin)
|
||||
}
|
||||
|
||||
data = Decoder7Bit(data[2:])
|
||||
|
||||
addresses := make([]onewire.Address, len(data)/8)
|
||||
for i := range addresses {
|
||||
addresses[i] = onewire.Address(binary.LittleEndian.Uint64(data[i*8:]))
|
||||
}
|
||||
return addresses, nil
|
||||
}
|
||||
|
||||
func (b *OneWireBus) Tx(w, r []byte, power onewire.Pullup) error {
|
||||
if len(w) == 0 && len(r) == 0 {
|
||||
return nil
|
||||
}
|
||||
|
||||
if len(r) > math.MaxUint16 {
|
||||
return fmt.Errorf("%w: cannot reach more than %d bytes", ErrValueOutOfRange, math.MaxUint16)
|
||||
}
|
||||
|
||||
b.mu.Lock()
|
||||
defer b.mu.Unlock()
|
||||
|
||||
ch := make(chan []byte)
|
||||
defer close(ch)
|
||||
|
||||
release, err := b.c.SetOneWireListener(b.q.pin, ch)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
defer release()
|
||||
|
||||
var powerVal byte = 0x00
|
||||
if power {
|
||||
powerVal = 0x01
|
||||
}
|
||||
|
||||
if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionConfigure), b.q.pin, powerVal); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
cmd := OneWireCommandReset
|
||||
|
||||
if len(w) > 0 {
|
||||
switch w[0] {
|
||||
case 0xF0: // search rom
|
||||
if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearch), b.q.pin); err != nil {
|
||||
return err
|
||||
}
|
||||
case 0xEC: // search rom (alarmed)
|
||||
if _, err := b.c.SendSysEx(SysExOneWireData, byte(OneWireInstructionSearchAlarmed), b.q.pin); err != nil {
|
||||
return err
|
||||
}
|
||||
case 0xCC: // skip rom
|
||||
panic("not implemented")
|
||||
case 0x33: // read rom
|
||||
panic("not implemented")
|
||||
case 0x55: // match rom
|
||||
cmd |= OneWireCommandSelect
|
||||
|
||||
payload := w[1:9]
|
||||
|
||||
if len(w) > 9 { // command length (1) + address length (8)
|
||||
cmd |= OneWireCommandWrite
|
||||
}
|
||||
if len(r) > 0 {
|
||||
cmd |= OneWireCommandRead
|
||||
// Write the amount of bytes to read.
|
||||
payload = append(payload, byte(len(r)&0xFF), byte((len(r)>>8)&0xFF))
|
||||
// Write the correlation id for sanity checking.
|
||||
payload = append(payload, byte(b.cid&0xFF), byte((b.cid>>8)&0xFF))
|
||||
b.cid++
|
||||
}
|
||||
|
||||
cmd |= OneWireCommandDelay
|
||||
payload = append(payload, 10, 0, 0, 0)
|
||||
|
||||
payload = append(payload, w[9:]...)
|
||||
payload = append([]byte{byte(cmd), b.q.pin}, Encoder7Bit(payload)...)
|
||||
if _, err := b.c.SendSysEx(SysExOneWireData, payload...); err != nil {
|
||||
return err
|
||||
}
|
||||
default:
|
||||
return fmt.Errorf("%w: no onewire command matching 0x%02X", ErrUnsupportedFeature, w[0])
|
||||
}
|
||||
}
|
||||
|
||||
if len(r) > 0 {
|
||||
data := <-ch
|
||||
|
||||
ins := OneWireInstruction(data[0])
|
||||
pin := data[1]
|
||||
|
||||
if ins != OneWireInstructionReadReply {
|
||||
return fmt.Errorf("%w: did not receive read reply", ErrUnhandledMessage)
|
||||
}
|
||||
if pin != b.q.pin {
|
||||
return fmt.Errorf("%w: received message from wrong bus", ErrInvalidOneWirePin)
|
||||
}
|
||||
|
||||
data = Decoder7Bit(data[2:])
|
||||
|
||||
// Drop the correlation id when copying.
|
||||
copy(r, data[2:])
|
||||
}
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (b *OneWireBus) Q() gpio.PinIO {
|
||||
return b.q
|
||||
}
|
||||
@ -0,0 +1,27 @@
|
||||
package firmata
|
||||
|
||||
type OneWireInstruction uint8
|
||||
|
||||
const (
|
||||
OneWireInstructionSearch OneWireInstruction = 0x40
|
||||
OneWireInstructionConfigure OneWireInstruction = 0x41
|
||||
OneWireInstructionSearchReply OneWireInstruction = 0x42
|
||||
OneWireInstructionReadReply OneWireInstruction = 0x43
|
||||
OneWireInstructionSearchAlarmed OneWireInstruction = 0x44
|
||||
OneWireInstructionSearchAlarmedReply OneWireInstruction = 0x45
|
||||
)
|
||||
|
||||
type OneWireConnectorWrapper struct {
|
||||
client ClientI
|
||||
}
|
||||
|
||||
type OneWireCommand uint8
|
||||
|
||||
const (
|
||||
OneWireCommandReset OneWireCommand = 0b00000001
|
||||
OneWireCommandSkip OneWireCommand = 0b00000010
|
||||
OneWireCommandSelect OneWireCommand = 0b00000100
|
||||
OneWireCommandRead OneWireCommand = 0b00001000
|
||||
OneWireCommandDelay OneWireCommand = 0b00010000
|
||||
OneWireCommandWrite OneWireCommand = 0b00100000
|
||||
)
|
||||
@ -0,0 +1,233 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"context"
|
||||
"errors"
|
||||
"sync"
|
||||
"time"
|
||||
|
||||
"periph.io/x/conn/v3/gpio"
|
||||
"periph.io/x/conn/v3/physic"
|
||||
"periph.io/x/conn/v3/pin"
|
||||
)
|
||||
|
||||
var (
|
||||
ErrUnsupportedGPIOPull = errors.New("firmata: PullDown is not supported")
|
||||
ErrNoMatchingGPIOPull = errors.New("firmata: pin was previously in a non-input mode")
|
||||
)
|
||||
|
||||
type Pin struct {
|
||||
c ClientI
|
||||
pin uint8
|
||||
edge gpio.Edge
|
||||
ch chan gpio.Level
|
||||
release func()
|
||||
done chan struct{}
|
||||
valueLast gpio.Level
|
||||
valueNew gpio.Level
|
||||
edgeChange chan gpio.Edge
|
||||
mu sync.Mutex
|
||||
}
|
||||
|
||||
func newPin(c ClientI, num uint8) *Pin {
|
||||
p := &Pin{
|
||||
c: c,
|
||||
pin: num,
|
||||
ch: make(chan gpio.Level),
|
||||
}
|
||||
|
||||
go p.run()
|
||||
|
||||
return p
|
||||
}
|
||||
|
||||
func (p *Pin) run() {
|
||||
p.done = make(chan struct{})
|
||||
|
||||
for {
|
||||
select {
|
||||
case <-p.done:
|
||||
close(p.ch)
|
||||
return
|
||||
case v := <-p.ch:
|
||||
p.valueLast = p.valueNew
|
||||
p.valueNew = v
|
||||
|
||||
func() {
|
||||
p.mu.Lock()
|
||||
defer p.mu.Unlock()
|
||||
|
||||
if p.edgeChange == nil {
|
||||
return
|
||||
}
|
||||
|
||||
if p.valueLast == true || p.valueNew == false {
|
||||
p.edgeChange <- gpio.FallingEdge
|
||||
}
|
||||
if p.valueLast == false || p.valueNew == true {
|
||||
p.edgeChange <- gpio.RisingEdge
|
||||
}
|
||||
}()
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Pin) In(pull gpio.Pull, edge gpio.Edge) error {
|
||||
var mode = PinFuncDigitalInput
|
||||
switch pull {
|
||||
case gpio.PullDown:
|
||||
return ErrUnsupportedGPIOPull
|
||||
case gpio.PullNoChange:
|
||||
ch, err := p.c.PinStateQuery(p.pin)
|
||||
if err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
s := <-ch
|
||||
mode = s.Mode
|
||||
|
||||
switch mode {
|
||||
case PinFuncInputPullUp:
|
||||
case PinFuncDigitalInput:
|
||||
default:
|
||||
return ErrNoMatchingGPIOPull
|
||||
}
|
||||
case gpio.PullUp:
|
||||
mode = PinFuncInputPullUp
|
||||
case gpio.Float:
|
||||
mode = PinFuncDigitalInput
|
||||
}
|
||||
|
||||
if err := p.c.SetPinMode(p.pin, mode); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
var err error
|
||||
if p.release, err = p.c.SetDigitalIOMessageListener(p.pin, p.ch); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
if err = p.c.SetDigitalPinReporting(p.pin, true); err != nil {
|
||||
return err
|
||||
}
|
||||
|
||||
p.edge = edge
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (p *Pin) Read() gpio.Level {
|
||||
return p.valueNew
|
||||
}
|
||||
|
||||
func (p *Pin) WaitForEdge(timeout time.Duration) bool {
|
||||
defer func() {
|
||||
p.mu.Lock()
|
||||
defer p.mu.Unlock()
|
||||
|
||||
close(p.edgeChange)
|
||||
p.edgeChange = nil
|
||||
}()
|
||||
|
||||
func() {
|
||||
p.mu.Lock()
|
||||
defer p.mu.Unlock()
|
||||
|
||||
p.edgeChange = make(chan gpio.Edge)
|
||||
}()
|
||||
|
||||
ctx, cancel := context.WithTimeout(context.Background(), timeout)
|
||||
defer cancel()
|
||||
|
||||
for {
|
||||
select {
|
||||
case change := <-p.edgeChange:
|
||||
if p.edge == gpio.BothEdges || change == p.edge {
|
||||
return true
|
||||
}
|
||||
case <-ctx.Done():
|
||||
return false
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (p *Pin) Pull() gpio.Pull {
|
||||
ch, err := p.c.PinStateQuery(p.pin)
|
||||
if err != nil {
|
||||
return gpio.PullNoChange
|
||||
}
|
||||
|
||||
s := <-ch
|
||||
switch s.Mode {
|
||||
case PinFuncInputPullUp:
|
||||
return gpio.PullUp
|
||||
case PinFuncDigitalInput:
|
||||
return gpio.Float
|
||||
}
|
||||
|
||||
return gpio.PullNoChange
|
||||
}
|
||||
|
||||
func (p *Pin) DefaultPull() gpio.Pull {
|
||||
return gpio.PullNoChange
|
||||
}
|
||||
|
||||
func (p *Pin) Out(l gpio.Level) error {
|
||||
// TODO:
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (p *Pin) PWM(duty gpio.Duty, f physic.Frequency) error {
|
||||
// TODO:
|
||||
panic("implement me")
|
||||
}
|
||||
|
||||
func (p *Pin) Func() pin.Func {
|
||||
ch, err := p.c.PinStateQuery(p.pin)
|
||||
if err != nil {
|
||||
return pin.FuncNone
|
||||
}
|
||||
|
||||
s := <-ch
|
||||
|
||||
return s.Mode
|
||||
}
|
||||
|
||||
func (p *Pin) SetFunc(f pin.Func) error {
|
||||
return p.c.SetPinMode(p.pin, f)
|
||||
}
|
||||
|
||||
func (p *Pin) SupportedFuncs() []pin.Func {
|
||||
return p.c.GetPinFunctions(p.pin)
|
||||
}
|
||||
|
||||
func (p *Pin) Halt() error {
|
||||
close(p.done)
|
||||
p.release()
|
||||
if err := p.c.SetDigitalPinReporting(p.pin, false); err != nil {
|
||||
return err
|
||||
}
|
||||
if err := p.c.SetAnalogPinReporting(p.pin, false); err != nil {
|
||||
return err
|
||||
}
|
||||
if err := p.c.SetDigitalPinValue(p.pin, gpio.Low); err != nil {
|
||||
return err
|
||||
}
|
||||
return nil
|
||||
}
|
||||
|
||||
func (p *Pin) Name() string {
|
||||
return p.c.GetPinName(p.pin)
|
||||
}
|
||||
|
||||
func (p *Pin) String() string {
|
||||
return p.Name()
|
||||
}
|
||||
|
||||
func (p *Pin) Number() int {
|
||||
return int(p.pin)
|
||||
}
|
||||
|
||||
func (p *Pin) Function() string {
|
||||
return string(p.Func())
|
||||
}
|
||||
@ -0,0 +1,62 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"periph.io/x/conn/v3/pin"
|
||||
)
|
||||
|
||||
const (
|
||||
PinFuncDigitalInput pin.Func = "DigitalInput"
|
||||
PinFuncDigitalOutput pin.Func = "DigitalOutput"
|
||||
PinFuncAnalogInput pin.Func = "AnalogInput"
|
||||
PinFuncPWM pin.Func = "PWM"
|
||||
PinFuncServo pin.Func = "Servo"
|
||||
PinFuncShift pin.Func = "Shift"
|
||||
PinFuncI2C pin.Func = "I2C"
|
||||
PinFuncOneWire pin.Func = "OneWire"
|
||||
PinFuncStepper pin.Func = "Stepper"
|
||||
PinFuncEncoder pin.Func = "Encoder"
|
||||
PinFuncSerial pin.Func = "Serial"
|
||||
PinFuncInputPullUp pin.Func = "InputPullUp"
|
||||
PinFuncSPI pin.Func = "SPI"
|
||||
PinFuncSonar pin.Func = "Sonar"
|
||||
PinFuncTone pin.Func = "Tone"
|
||||
PinFuncDHT pin.Func = "DHT"
|
||||
)
|
||||
|
||||
var pinFuncToModeMap = map[pin.Func]uint8{
|
||||
PinFuncDigitalInput: 0x0,
|
||||
PinFuncDigitalOutput: 0x1,
|
||||
PinFuncAnalogInput: 0x2,
|
||||
PinFuncPWM: 0x3,
|
||||
PinFuncServo: 0x4,
|
||||
PinFuncShift: 0x5,
|
||||
PinFuncI2C: 0x6,
|
||||
PinFuncOneWire: 0x7,
|
||||
PinFuncStepper: 0x8,
|
||||
PinFuncEncoder: 0x9,
|
||||
PinFuncSerial: 0xA,
|
||||
PinFuncInputPullUp: 0xB,
|
||||
PinFuncSPI: 0xC,
|
||||
PinFuncSonar: 0xD,
|
||||
PinFuncTone: 0xE,
|
||||
PinFuncDHT: 0xF,
|
||||
}
|
||||
|
||||
var pinModeToFuncMap = map[uint8]pin.Func{
|
||||
0x0: PinFuncDigitalInput,
|
||||
0x1: PinFuncDigitalOutput,
|
||||
0x2: PinFuncAnalogInput,
|
||||
0x3: PinFuncPWM,
|
||||
0x4: PinFuncServo,
|
||||
0x5: PinFuncShift,
|
||||
0x6: PinFuncI2C,
|
||||
0x7: PinFuncOneWire,
|
||||
0x8: PinFuncStepper,
|
||||
0x9: PinFuncEncoder,
|
||||
0xA: PinFuncSerial,
|
||||
0xB: PinFuncInputPullUp,
|
||||
0xC: PinFuncSPI,
|
||||
0xD: PinFuncSonar,
|
||||
0xE: PinFuncTone,
|
||||
0xF: PinFuncDHT,
|
||||
}
|
||||
@ -0,0 +1,17 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
|
||||
"periph.io/x/conn/v3/pin"
|
||||
)
|
||||
|
||||
type PinStateResponse struct {
|
||||
Pin uint8
|
||||
Mode pin.Func
|
||||
State int
|
||||
}
|
||||
|
||||
func (p PinStateResponse) String() string {
|
||||
return fmt.Sprintf("pin(%d) mode(%s) state(%d)", p.Pin, p.Mode, p.State)
|
||||
}
|
||||
@ -0,0 +1,109 @@
|
||||
package firmata
|
||||
|
||||
type (
|
||||
SysExCmd uint8
|
||||
SysExExtendedCmd uint16
|
||||
)
|
||||
|
||||
// Base Features
|
||||
const (
|
||||
SysExExtendedId SysExCmd = 0x00 // A value of 0x00 indicates the next 2 bytes define the extended ID
|
||||
SysExAnalogMappingQuery SysExCmd = 0x69 // ask for mapping of analog pin names to pin numbers
|
||||
SysExAnalogMappingResponse SysExCmd = 0x6A // reply with mapping info
|
||||
SysExCapabilityQuery SysExCmd = 0x6B // ask for supported modes and resolution of all pins
|
||||
SysExCapabilityResponse SysExCmd = 0x6C // reply with supported modes and resolution
|
||||
SysExPinStateQuery SysExCmd = 0x6D // ask for a pin's current mode and state (different from value)
|
||||
SysExPinStateResponse SysExCmd = 0x6E // reply with a pin's current mode and state (different from value)
|
||||
SysExExtendedAnalog SysExCmd = 0x6F // analog write (PWM, Servo, etc.) to any pin
|
||||
SysExStringData SysExCmd = 0x71 // a string message with 14-bits per char
|
||||
SysExReportFirmware SysExCmd = 0x79 // report name and version of the firmware
|
||||
SysExSamplingInterval SysExCmd = 0x7A // the interval at which analog input is sampled (default = 19ms)
|
||||
SysExNonRealtime SysExCmd = 0x7E // MIDI Reserved for non-realtime messages
|
||||
SysExRealtime SysExCmd = 0x7F // MIDI Reserved for realtime messages
|
||||
)
|
||||
|
||||
// User Defined Feature Codes
|
||||
// Assign these to whatever feature constant you define
|
||||
const (
|
||||
UserFeature1 SysExCmd = 0x01
|
||||
UserFeature2 SysExCmd = 0x02
|
||||
UserFeature3 SysExCmd = 0x03
|
||||
UserFeature4 SysExCmd = 0x04
|
||||
UserFeature5 SysExCmd = 0x05
|
||||
UserFeature6 SysExCmd = 0x06
|
||||
UserFeature7 SysExCmd = 0x07
|
||||
UserFeature8 SysExCmd = 0x08
|
||||
UserFeature9 SysExCmd = 0x09
|
||||
UserFeatureA SysExCmd = 0x0A
|
||||
UserFeatureB SysExCmd = 0x0B
|
||||
UserFeatureC SysExCmd = 0x0C
|
||||
UserFeatureD SysExCmd = 0x0D
|
||||
UserFeatureE SysExCmd = 0x0E
|
||||
UserFeatureF SysExCmd = 0x0F
|
||||
)
|
||||
|
||||
// Optionally Included Features
|
||||
const (
|
||||
SysExRCOutputData SysExCmd = 0x5C // https://github.com/firmata/protocol/blob/master/proposals/rcswitch-proposal.md
|
||||
SysExRCInputData SysExCmd = 0x5D // https://github.com/firmata/protocol/blob/master/proposals/rcswitch-proposal.md
|
||||
SysExDeviceQuery SysExCmd = 0x5E // https://github.com/finson-release/Luni/blob/master/extras/v0.9/v0.8-device-driver-C-firmata-messages.md
|
||||
SysExDeviceResponse SysExCmd = 0x5F // https://github.com/finson-release/Luni/blob/master/extras/v0.9/v0.8-device-driver-C-firmata-messages.md
|
||||
SysExSerialDataV1 SysExCmd = 0x60 // https://github.com/firmata/protocol/blob/master/serial-1.0.md
|
||||
SysExEncoderData SysExCmd = 0x61 // https://github.com/firmata/protocol/blob/master/encoder.md
|
||||
SysExAccelStepperData SysExCmd = 0x62 // https://github.com/firmata/protocol/blob/master/accelStepperFirmata.md
|
||||
SysExSerialDataV2 SysExCmd = 0x67 // https://github.com/firmata/protocol/blob/master/proposals/serial-2.0-proposal.md
|
||||
SysExSPIData SysExCmd = 0x68 // https://github.com/firmata/protocol/blob/master/spi.md
|
||||
SysExServoConfig SysExCmd = 0x70 // https://github.com/firmata/protocol/blob/master/servos.md
|
||||
SysExStepperData SysExCmd = 0x72 // https://github.com/firmata/protocol/blob/master/stepper-legacy.md
|
||||
SysExOneWireData SysExCmd = 0x73 // https://github.com/firmata/protocol/blob/master/onewire.md
|
||||
SysExDHTSensorData SysExCmd = 0x74 // https://github.com/firmata/protocol/blob/master/dhtsensor.md
|
||||
SysExShiftData SysExCmd = 0x75 // https://github.com/firmata/protocol/blob/master/proposals/shift-proposal.md
|
||||
SysExI2CRequest SysExCmd = 0x76 // https://github.com/firmata/protocol/blob/master/i2c.md
|
||||
SysExI2CReply SysExCmd = 0x77 // https://github.com/firmata/protocol/blob/master/i2c.md
|
||||
SysExI2CConfig SysExCmd = 0x78 // https://github.com/firmata/protocol/blob/master/i2c.md
|
||||
SysExSchedulerData SysExCmd = 0x7B // https://github.com/firmata/protocol/blob/master/scheduler.md
|
||||
SysExFrequencyCommand SysExCmd = 0x7D // https://github.com/firmata/protocol/blob/master/frequency.md
|
||||
)
|
||||
|
||||
var sysExCmdToStringMap = map[SysExCmd]string{
|
||||
SysExExtendedId: "ExtendedId",
|
||||
SysExAnalogMappingQuery: "AnalogMappingQuery",
|
||||
SysExAnalogMappingResponse: "AnalogMappingResponse",
|
||||
SysExCapabilityQuery: "CapabilityQuery",
|
||||
SysExCapabilityResponse: "CapabilityResponse",
|
||||
SysExPinStateQuery: "PinStateQuery",
|
||||
SysExPinStateResponse: "PinStateResponse",
|
||||
SysExExtendedAnalog: "ExtendedAnalog",
|
||||
SysExStringData: "StringData",
|
||||
SysExReportFirmware: "ReportFirmware",
|
||||
SysExSamplingInterval: "SamplingInterval",
|
||||
SysExNonRealtime: "NonRealtime",
|
||||
SysExRealtime: "Realtime",
|
||||
SysExRCOutputData: "RCOutputData",
|
||||
SysExRCInputData: "RCInputData",
|
||||
SysExDeviceQuery: "DeviceQuery",
|
||||
SysExDeviceResponse: "DeviceResponse",
|
||||
SysExSerialDataV1: "SerialDataV1",
|
||||
SysExEncoderData: "EncoderData",
|
||||
SysExAccelStepperData: "AccelStepperData",
|
||||
SysExSerialDataV2: "SerialDataV2",
|
||||
SysExSPIData: "SPIData",
|
||||
SysExServoConfig: "ServoConfig",
|
||||
SysExStepperData: "StepperData",
|
||||
SysExOneWireData: "OneWireData",
|
||||
SysExDHTSensorData: "DHTSensorData",
|
||||
SysExShiftData: "ShiftData",
|
||||
SysExI2CRequest: "I2CRequest",
|
||||
SysExI2CReply: "I2CReply",
|
||||
SysExI2CConfig: "I2CConfig",
|
||||
SysExSchedulerData: "SchedulerData",
|
||||
SysExFrequencyCommand: "FrequencyCommand",
|
||||
}
|
||||
|
||||
func (s SysExCmd) String() string {
|
||||
if v, ok := sysExCmdToStringMap[s]; ok {
|
||||
return v
|
||||
}
|
||||
|
||||
return "Unknown"
|
||||
}
|
||||
@ -0,0 +1,114 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
)
|
||||
|
||||
const SevenBitMask byte = 0b01111111
|
||||
|
||||
func TwoByteToByte(a, b byte) byte {
|
||||
return (a & SevenBitMask) | ((b & SevenBitMask) << 7)
|
||||
}
|
||||
|
||||
func TwoByteToWord(a, b byte) uint16 {
|
||||
return uint16(a&SevenBitMask) | (uint16(b&SevenBitMask) << 7)
|
||||
}
|
||||
|
||||
func TwoByteString(bytes []byte) string {
|
||||
if len(bytes)%2 == 1 {
|
||||
bytes = append(bytes, 0)
|
||||
}
|
||||
|
||||
var s string
|
||||
for i := 0; i < len(bytes); i += 2 {
|
||||
s += string(TwoByteToByte(bytes[i], bytes[i+1]))
|
||||
}
|
||||
return s
|
||||
}
|
||||
|
||||
func TwoByteRepresentationToByteSlice(bytes []byte) []byte {
|
||||
if len(bytes)%2 == 1 {
|
||||
bytes = append(bytes, 0)
|
||||
}
|
||||
|
||||
d := make([]byte, len(bytes)/2)
|
||||
i := 0
|
||||
for di := range d {
|
||||
d[di] = TwoByteToByte(bytes[i], bytes[i+1])
|
||||
i += 2
|
||||
}
|
||||
return d
|
||||
}
|
||||
|
||||
func ByteToTwoByte(b byte) (lsb, msb byte) {
|
||||
return b & SevenBitMask, (b >> 7) & SevenBitMask
|
||||
}
|
||||
|
||||
func WordToTwoByte(b uint16) (lsb, msb byte) {
|
||||
return byte(b) & SevenBitMask, byte(b>>7) & SevenBitMask
|
||||
}
|
||||
|
||||
func ByteSliceToTwoByteRepresentation(bytes []byte) []byte {
|
||||
d := make([]byte, len(bytes)*2)
|
||||
i := 0
|
||||
for _, b := range bytes {
|
||||
d[i], d[i+1] = ByteToTwoByte(b)
|
||||
i += 2
|
||||
}
|
||||
return d
|
||||
}
|
||||
|
||||
func SprintHexArray(data []byte) string {
|
||||
s := ""
|
||||
if len(data) == 0 {
|
||||
return s
|
||||
}
|
||||
for _, b := range data {
|
||||
s += fmt.Sprintf("0x%02X ", b)
|
||||
}
|
||||
return s[:len(s)-1]
|
||||
}
|
||||
|
||||
// Encoder7Bit logic determined from here:
|
||||
// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.cpp:34
|
||||
func Encoder7Bit(inData []byte) []byte {
|
||||
var outData []byte
|
||||
var previous byte
|
||||
var shift = 0
|
||||
for _, data := range inData {
|
||||
if shift == 0 {
|
||||
outData = append(outData, data&0x7f)
|
||||
shift++
|
||||
previous = data >> 7
|
||||
} else {
|
||||
outData = append(outData, ((data<<shift)&0x7f)|previous)
|
||||
if shift == 6 {
|
||||
outData = append(outData, data>>1)
|
||||
shift = 0
|
||||
} else {
|
||||
shift++
|
||||
previous = data >> (8 - shift)
|
||||
}
|
||||
}
|
||||
}
|
||||
if shift > 0 {
|
||||
outData = append(outData, previous)
|
||||
}
|
||||
return outData
|
||||
}
|
||||
|
||||
// Decoder7Bit logic determined from here:
|
||||
// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.h:17
|
||||
// - ConfigurableFirmata@2.10.1/src/Encoder7Bit.cpp:54
|
||||
func Decoder7Bit(inData []byte) []byte {
|
||||
var outBytes = ((len(inData)) * 7) >> 3
|
||||
|
||||
var outData = make([]byte, outBytes)
|
||||
for i := 0; i < outBytes; i++ {
|
||||
var j = i << 3
|
||||
var pos = j / 7
|
||||
var shift = byte(j % 7)
|
||||
outData[i] = (inData[pos] >> shift) | ((inData[pos+1] << (7 - shift)) & 0xFF)
|
||||
}
|
||||
return outData
|
||||
}
|
||||
@ -0,0 +1,129 @@
|
||||
package firmata
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"reflect"
|
||||
"testing"
|
||||
)
|
||||
|
||||
func TestByteConversion(t *testing.T) {
|
||||
for i := uint16(0x00); i <= 0xFF; i++ {
|
||||
t.Run(fmt.Sprintf("0x%02X", i), func(t *testing.T) {
|
||||
a, b := ByteToTwoByte(byte(i))
|
||||
o := TwoByteToByte(a, b)
|
||||
if byte(i) != o {
|
||||
t.Errorf("ByteToTwoByte(0x%02X) = 0x%02X, 0x%02X => TwoByteToByte() = 0x%02X", i, a, b, o)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestTwoByteString(t *testing.T) {
|
||||
tests := []struct {
|
||||
name string
|
||||
bytes []byte
|
||||
want string
|
||||
}{
|
||||
{
|
||||
name: "nil",
|
||||
bytes: nil,
|
||||
want: "",
|
||||
},
|
||||
{
|
||||
name: "empty",
|
||||
bytes: []byte{},
|
||||
want: "",
|
||||
},
|
||||
{
|
||||
name: "test string",
|
||||
bytes: ByteSliceToTwoByteRepresentation([]byte{
|
||||
0x74, 0x65, 0x73, 0x74, 0x20, 0x73, 0x74, 0x72, 0x69, 0x6E, 0x67,
|
||||
}),
|
||||
want: "test string",
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
if got := TwoByteString(tt.bytes); got != tt.want {
|
||||
t.Errorf("TwoByteString() = %v, want %v", got, tt.want)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestByteSliceTo2ByteRepresentation(t *testing.T) {
|
||||
tests := []struct {
|
||||
name string
|
||||
input []byte
|
||||
expected []byte
|
||||
}{
|
||||
{
|
||||
name: "nil",
|
||||
input: nil,
|
||||
expected: []byte{},
|
||||
},
|
||||
{
|
||||
name: "empty",
|
||||
input: []byte{},
|
||||
expected: []byte{},
|
||||
},
|
||||
{
|
||||
name: "7 lsb set",
|
||||
input: []byte{0b01111111, 0b11111111},
|
||||
expected: []byte{0b01111111, 0b00000000, 0b01111111, 0b00000001},
|
||||
},
|
||||
{
|
||||
name: "7 lsb not set",
|
||||
input: []byte{0b00000000, 0b10000000},
|
||||
expected: []byte{0b00000000, 0b00000000, 0b00000000, 0b00000001},
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
if got := ByteSliceToTwoByteRepresentation(tt.input); !reflect.DeepEqual(got, tt.expected) {
|
||||
t.Errorf("ByteSliceToTwoByteRepresentation() = %v, want %v", got, tt.expected)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
|
||||
func TestTwoByteRepresentationToByteSlice(t *testing.T) {
|
||||
tests := []struct {
|
||||
name string
|
||||
input []byte
|
||||
expected []byte
|
||||
}{
|
||||
{
|
||||
name: "nil",
|
||||
input: nil,
|
||||
expected: []byte{},
|
||||
},
|
||||
{
|
||||
name: "empty",
|
||||
input: []byte{},
|
||||
expected: []byte{},
|
||||
},
|
||||
{
|
||||
name: "7 lsb set",
|
||||
input: []byte{0b01111111, 0b00000000, 0b01111111, 0b00000001},
|
||||
expected: []byte{0b01111111, 0b11111111},
|
||||
},
|
||||
{
|
||||
name: "7 lsb not set",
|
||||
input: []byte{0b00000000, 0b00000000, 0b00000000, 0b00000001},
|
||||
expected: []byte{0b00000000, 0b10000000},
|
||||
},
|
||||
{
|
||||
name: "only 1 byte",
|
||||
input: []byte{0b01000000},
|
||||
expected: []byte{0b01000000},
|
||||
},
|
||||
}
|
||||
for _, tt := range tests {
|
||||
t.Run(tt.name, func(t *testing.T) {
|
||||
if got := TwoByteRepresentationToByteSlice(tt.input); !reflect.DeepEqual(got, tt.expected) {
|
||||
t.Errorf("TwoByteRepresentationToByteSlice() = %v, want %v", got, tt.expected)
|
||||
}
|
||||
})
|
||||
}
|
||||
}
|
||||
Loading…
Reference in New Issue