forked from mohr/ArduinoIO
Compare commits
3 Commits
5f32dbb595
...
SPIFunctio
Author | SHA1 | Date | |
---|---|---|---|
8536d04868 | |||
39c171c186 | |||
3141c7a858 |
1
.gitignore
vendored
1
.gitignore
vendored
@ -103,4 +103,3 @@ local.properties
|
|||||||
.cache-main
|
.cache-main
|
||||||
.scala_dependencies
|
.scala_dependencies
|
||||||
.worksheet
|
.worksheet
|
||||||
/Release/
|
|
||||||
|
4
.gitmodules
vendored
4
.gitmodules
vendored
@ -1,3 +1,3 @@
|
|||||||
[submodule "arduino"]
|
[submodule "arduino_core"]
|
||||||
path = arduino_core
|
path = arduino_core
|
||||||
url = https://egit.irs.uni-stuttgart.de/eive/arduino_core.git
|
url = https://egit.irs.uni-stuttgart.de/mohr/arduino_core.git
|
||||||
|
@ -1,37 +0,0 @@
|
|||||||
#ifndef ARDUINOCONFIG_H_
|
|
||||||
#define ARDUINOCONFIG_H_
|
|
||||||
|
|
||||||
static const uint8_t COMMAND_TRANSFER_SPI = 1;
|
|
||||||
|
|
||||||
// Can be set to one for additional programming information from the primary
|
|
||||||
// serial port which is usually also used to flash the Arduino
|
|
||||||
// Can be disabled if this output interferes with the usual
|
|
||||||
// serial communication logic
|
|
||||||
#define PROGRAMMING_OUTPUT 1
|
|
||||||
|
|
||||||
#define BAUD_RATE 115200
|
|
||||||
#define SERIAL_RX_BUFFER_SIZE 256
|
|
||||||
#define RING_BUFFER_SIZE 100
|
|
||||||
#define MAX_PACKET_LENGTH 100
|
|
||||||
#define RING_BUFFER_CHECK_INTVL 1000
|
|
||||||
|
|
||||||
// Define which port to use for the SPI Chip Select by using the register
|
|
||||||
// definitions. The data direction register is assigned as well.
|
|
||||||
// The ports can be looked up on the official Arduino pinout schematics.
|
|
||||||
#ifdef ARDUINO_AVR_MEGA2560
|
|
||||||
// Defines for the Arduino Mega
|
|
||||||
#define CS_PORT PORTK
|
|
||||||
#define CS_DDR DDRK
|
|
||||||
#elif defined(ARDUINO_AVR_UNO)
|
|
||||||
#define CS_PORT PORTD
|
|
||||||
#define CS_DDR DDRD
|
|
||||||
#elif defined(__SAM3X8E__)
|
|
||||||
// Define for the Arduino Due
|
|
||||||
#define CS_PORT PORTC
|
|
||||||
#define CS_DDR DDRC
|
|
||||||
#else
|
|
||||||
#define CS_PORT PORTC
|
|
||||||
#define CS_DDR DDRC
|
|
||||||
#endif
|
|
||||||
|
|
||||||
#endif /* ARDUINOCONFIG_H_ */
|
|
172
IOBoard.cpp
172
IOBoard.cpp
@ -1,172 +0,0 @@
|
|||||||
#include "IOBoard.h"
|
|
||||||
#include <SPI.h>
|
|
||||||
#include "ArduinoConfig.h"
|
|
||||||
#include "helper/SimpleRingBuffer.h"
|
|
||||||
#include "helper/DleEncoder.h"
|
|
||||||
#include "helper/crc_ccitt.h"
|
|
||||||
|
|
||||||
SimpleRingBuffer ringBuffer(RING_BUFFER_SIZE, true);
|
|
||||||
|
|
||||||
uint8_t rawData[2 * RING_BUFFER_SIZE];
|
|
||||||
size_t rawDataSize = 0;
|
|
||||||
|
|
||||||
#define PACKET_COMMAND_LENGTH 1
|
|
||||||
#define PACKET_ADDRESS_LENGTH 1
|
|
||||||
#define PACKET_SIZE_FIELD_LENGTH 2
|
|
||||||
|
|
||||||
#define PACKET_HEADER_LENGTH 4
|
|
||||||
|
|
||||||
namespace IOBoard {
|
|
||||||
|
|
||||||
void handleNewData() {
|
|
||||||
ringBuffer.readData(rawData, sizeof(rawData), true, &rawDataSize);
|
|
||||||
|
|
||||||
if (rawDataSize == 0) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
//look for STX
|
|
||||||
size_t firstSTXinRawData = 0;
|
|
||||||
while ((firstSTXinRawData < rawDataSize)
|
|
||||||
&& (rawData[firstSTXinRawData] != DleEncoder::STX)) {
|
|
||||||
Serial.println(rawData[firstSTXinRawData]);
|
|
||||||
firstSTXinRawData++;
|
|
||||||
}
|
|
||||||
|
|
||||||
if (rawData[firstSTXinRawData] != DleEncoder::STX) {
|
|
||||||
//there is no STX in our data, throw it away...
|
|
||||||
Serial.println("NO STX");
|
|
||||||
ringBuffer.deleteData(rawDataSize);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t packet[MAX_PACKET_LENGTH];
|
|
||||||
size_t packetLen;
|
|
||||||
|
|
||||||
size_t readSize;
|
|
||||||
|
|
||||||
ReturnValue_t result = DleEncoder::decode(rawData + firstSTXinRawData,
|
|
||||||
rawDataSize - firstSTXinRawData, &readSize, packet, sizeof(packet),
|
|
||||||
&packetLen);
|
|
||||||
|
|
||||||
size_t toDelete = firstSTXinRawData;
|
|
||||||
if (result == HasReturnvaluesIF::RETURN_OK) {
|
|
||||||
handlePacket(packet, packetLen);
|
|
||||||
|
|
||||||
// after handling the packet, we can delete it from the raw stream,
|
|
||||||
// it has been copied to packet
|
|
||||||
toDelete += readSize;
|
|
||||||
}
|
|
||||||
|
|
||||||
//remove Data which was processed
|
|
||||||
ringBuffer.deleteData(toDelete);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void handlePacket(uint8_t *packet, size_t packetLen) {
|
|
||||||
/* Paket layout is:
|
|
||||||
------------------------------
|
|
||||||
|byte | field |
|
|
||||||
| | |
|
|
||||||
|------------------------------|
|
|
||||||
|1 | 8 bit command |
|
|
||||||
|------------------------------|
|
|
||||||
|1 | 8 bit address |
|
|
||||||
|------------------------------|
|
|
||||||
|2 | 16bit length |
|
|
||||||
|------------------------------|
|
|
||||||
|length | <length> byte data |
|
|
||||||
|------------------------------|
|
|
||||||
|2 | 16 bit crc |
|
|
||||||
|------------------------------|
|
|
||||||
------------------------------
|
|
||||||
*/
|
|
||||||
|
|
||||||
uint16_t crc = Calculate_CRC(packet, packetLen);
|
|
||||||
|
|
||||||
if (crc != 0) {
|
|
||||||
Serial.println("-AI- Invalid packet checksum detected!");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint16_t payloadLen = (packet[2] << 8) | packet[3];
|
|
||||||
|
|
||||||
if (payloadLen != packetLen - 6) {
|
|
||||||
Serial.println("-AI- Invalid packet length detected!");
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
uint8_t command = packet[0];
|
|
||||||
uint8_t address = packet[1];
|
|
||||||
|
|
||||||
switch (command) {
|
|
||||||
case COMMAND_TRANSFER_SPI:
|
|
||||||
transferSPI(address, packet + PACKET_HEADER_LENGTH, payloadLen);
|
|
||||||
//echo the data back, no need to change the header fields, they are the same
|
|
||||||
//checksum will be written by sendReply()
|
|
||||||
//check reply:
|
|
||||||
Serial.println("Data response check: ");
|
|
||||||
for(size_t i =0; i< packetLen; i++){
|
|
||||||
Serial.print("packet nr ");Serial.print(i);Serial.print(" ");
|
|
||||||
Serial.println(packet[i]);
|
|
||||||
}
|
|
||||||
sendReply(packet, packetLen);
|
|
||||||
break;
|
|
||||||
default:
|
|
||||||
Serial.println("invalid command");
|
|
||||||
break;
|
|
||||||
}
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
void transferSPI(uint8_t address, uint8_t *data, size_t datalen) {
|
|
||||||
SPI.beginTransaction(SPISettings(14000000, MSBFIRST, SPI_MODE3));
|
|
||||||
// The specified address is the bit where the last bit is port 0
|
|
||||||
// and the first bit is port 7. It is inverted because the SPI protocol
|
|
||||||
// requires the slave select to be driven low.
|
|
||||||
CS_PORT = ~address;
|
|
||||||
SPI.transfer(data, datalen);
|
|
||||||
delay(100);
|
|
||||||
// Pull the slave select high again.
|
|
||||||
CS_PORT = 0xff;
|
|
||||||
SPI.endTransaction();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Encode and send the data, last two bytes of data are assumed to be for the checksum
|
|
||||||
* and will be overwritten with it
|
|
||||||
*/
|
|
||||||
void sendReply(uint8_t *data, size_t len) {
|
|
||||||
uint16_t crc = Calculate_CRC(data, len - 2);
|
|
||||||
data[len - 2] = crc >> 8;
|
|
||||||
data[len - 1] = crc;
|
|
||||||
//we're being conservative here
|
|
||||||
//TODO move this to global so other protocols can use it too
|
|
||||||
uint8_t buffer[2 * len + 2];
|
|
||||||
buffer[0] = DleEncoder::STX;
|
|
||||||
size_t writtenLen;
|
|
||||||
ReturnValue_t result = DleEncoder::encode(data, len, buffer, sizeof(buffer),
|
|
||||||
&writtenLen, true);
|
|
||||||
if (result != HasReturnvaluesIF::RETURN_OK) {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
Serial.write(buffer, writtenLen);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//TODO check if this is thread safe by arduino
|
|
||||||
// I think it is, see HardwareSerial::available()
|
|
||||||
void serialEvent() {
|
|
||||||
//Serial.println(ringBuffer.availableWriteSpace());
|
|
||||||
//uint8_t i = 0;
|
|
||||||
|
|
||||||
while (Serial.available()>0) {
|
|
||||||
uint8_t byte = Serial.read();
|
|
||||||
ringBuffer.writeData(&byte, 1);
|
|
||||||
}
|
|
||||||
//Serial.println(ringBuffer.availableWriteSpace());
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
17
IOBoard.h
17
IOBoard.h
@ -1,17 +0,0 @@
|
|||||||
#ifndef IOBOARD_H_
|
|
||||||
#define IOBOARD_H_
|
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
|
|
||||||
namespace IOBoard {
|
|
||||||
|
|
||||||
void handleNewData();
|
|
||||||
void handlePacket(uint8_t *packet, size_t packetLen);
|
|
||||||
void transferSPI(uint8_t address, uint8_t *data, size_t datalen);
|
|
||||||
void sendReply(uint8_t *data, size_t len);
|
|
||||||
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#endif /* IOBOARD_H_ */
|
|
19
Makefile
19
Makefile
@ -1,19 +0,0 @@
|
|||||||
TARGET = ArduinoIO
|
|
||||||
|
|
||||||
# The C source files
|
|
||||||
SRC =
|
|
||||||
|
|
||||||
# The C++ source files
|
|
||||||
PSRC += main.cpp
|
|
||||||
PSRC += IOBoard.cpp
|
|
||||||
PSRC += $(wildcard helper/*.cpp)
|
|
||||||
PSRC += arduino_core/ArduinoCore-avr/libraries/SPI/src/SPI.cpp
|
|
||||||
|
|
||||||
# extra Arduino libraries
|
|
||||||
ARDLIBS =
|
|
||||||
|
|
||||||
EXTRAINCDIRS += arduino_core/ArduinoCore-avr/libraries/SPI/src
|
|
||||||
EXTRAINCDIRS += arduino_core/src
|
|
||||||
|
|
||||||
# this line includes the Makefile.base
|
|
||||||
include arduino_core/arduino-base.mk
|
|
Submodule arduino_core updated: b9aa89f6d2...2e1a217737
204
main.cpp
204
main.cpp
@ -1,47 +1,187 @@
|
|||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
|
||||||
#include "IOBoard.h"
|
#include "helper/SimpleRingBuffer.h"
|
||||||
#include "ArduinoConfig.h"
|
#include "helper/DleEncoder.h"
|
||||||
|
#include "helper/crc_ccitt.h"
|
||||||
|
|
||||||
// TODO: Copy this header into arduino_core, so it can be used without
|
|
||||||
// Eclipse Sloeber as well.
|
|
||||||
#include <arduino-timer.h>
|
|
||||||
#include <avr/io.h>
|
#include <avr/io.h>
|
||||||
|
|
||||||
//SPI is formally a library, so it is not part of the objects compiled
|
//SPI is formally a library, so it is not part of the objects compiled
|
||||||
//from the core and we need to include it explicitly
|
//from the core and we need to include it explicitly
|
||||||
#include <SPI.h>
|
#include "arduino_core/ArduinoCore-avr/libraries/SPI/src/SPI.h"
|
||||||
|
|
||||||
auto timer = timer_create_default();
|
//Define which port to use for the SPI Chip Select
|
||||||
|
#define CS_PORT PORTB
|
||||||
|
#define CS_DDR DDRB
|
||||||
|
|
||||||
void setup() {
|
#define RING_BUFFER_SIZE 100
|
||||||
// Set data direction of selected port to output.
|
#define MAX_PACKET_LENGTH 100
|
||||||
CS_DDR = 0xff;
|
#define SERIAL_RX_BUFFER_SIZE 256
|
||||||
// Drive all the slave selects high as required for the SPI protocol
|
|
||||||
// if no transfer is going on.
|
static const uint8_t COMMAND_TRANSFER_SPI = 1;
|
||||||
CS_PORT = 0xff;
|
|
||||||
Serial.begin(BAUD_RATE);
|
SimpleRingBuffer ringBuffer(RING_BUFFER_SIZE, true);
|
||||||
#if PROGRAMMING_OUTPUT == 1
|
|
||||||
Serial.println("-AI- Setting up Arduino IO interface board.");
|
uint8_t rawData[2 * RING_BUFFER_SIZE];
|
||||||
Serial.print("-AI- Configured baud rate for serial communication: ");
|
size_t rawDataSize = 0;
|
||||||
Serial.println(BAUD_RATE, DEC);
|
|
||||||
Serial.print("-AI- Size of serial receiver buffer: ");
|
/**
|
||||||
Serial.print(SERIAL_RX_BUFFER_SIZE, DEC);
|
* Encode and send the data, last two bytes of data are assumed to be for the checksum
|
||||||
Serial.println(" bytes");
|
* and will be overwritten with it
|
||||||
#endif
|
*/
|
||||||
SPI.begin();
|
void sendData(uint8_t *data, size_t len) {
|
||||||
|
uint16_t crc = Calculate_CRC(data, len - 2);
|
||||||
|
data[len - 2] = crc >> 8;
|
||||||
|
data[len - 1] = crc;
|
||||||
|
//we're being conservative here
|
||||||
|
//TODO move this to global so other protocols can use it too
|
||||||
|
uint8_t buffer[2 * len + 2];
|
||||||
|
buffer[0] = DleEncoder::STX;
|
||||||
|
size_t writtenLen;
|
||||||
|
ReturnValue_t result = DleEncoder::encode(data, len, buffer, sizeof(buffer),
|
||||||
|
&writtenLen, true);
|
||||||
|
if (result != HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
Serial.write(buffer, writtenLen);
|
||||||
}
|
}
|
||||||
|
|
||||||
bool periodicHandler1(void* args) {
|
void transferSPI(uint8_t address, uint8_t *data, size_t datalen) {
|
||||||
if(args) {};
|
SPI.beginTransaction(SPISettings(100000, MSBFIRST, SPI_MODE3));
|
||||||
IOBoard::handleNewData();
|
//CS_PORT = ~address;
|
||||||
return false;
|
digitalWrite(10, LOW);
|
||||||
|
//included
|
||||||
|
for (size_t i = 0; i < datalen; i++)
|
||||||
|
{
|
||||||
|
//Serial.print("SentData is: ");Serial.println(data[i]);
|
||||||
|
data[i] = SPI.transfer(data[i]);
|
||||||
|
//Serial.print("Back from n-1 ");Serial.println(data[i]);
|
||||||
|
|
||||||
|
}
|
||||||
|
//SPI.transfer(data, datalen);
|
||||||
|
delay(100);
|
||||||
|
digitalWrite(10, HIGH);
|
||||||
|
//CS_PORT = 0xff;
|
||||||
|
SPI.endTransaction();
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void handlePacket(uint8_t *packet, size_t packetLen) {
|
||||||
|
//Paket layout is:
|
||||||
|
// 8 bit command | 8 bit address | 16bit length | <length> byte data | 16 bit crc
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
uint16_t crc = Calculate_CRC(packet, packetLen);
|
||||||
|
|
||||||
|
|
||||||
|
if (crc != 0) {
|
||||||
|
//Serial.println("invalid Checksum");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t payloadLen = (packet[2] << 8) | packet[3];
|
||||||
|
|
||||||
|
if (payloadLen != packetLen - 6) {
|
||||||
|
//Serial.println("invalid len");
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t command = packet[0];
|
||||||
|
uint8_t address = packet[1];
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
switch (command) {
|
||||||
|
case COMMAND_TRANSFER_SPI:
|
||||||
|
transferSPI(address, packet + 4, payloadLen);
|
||||||
|
//echo the data back, no need to change the header fields, they are the same
|
||||||
|
//checksum will be written by sendData()
|
||||||
|
//check reply:
|
||||||
|
|
||||||
|
sendData(packet, packetLen);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
//Serial.println("invalid command");
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
void handleNewData() {
|
||||||
|
ringBuffer.readData(rawData, sizeof(rawData), true, &rawDataSize);
|
||||||
|
|
||||||
|
if (rawDataSize == 0) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
//look for STX
|
||||||
|
size_t firstSTXinRawData = 0;
|
||||||
|
while ((firstSTXinRawData < rawDataSize)
|
||||||
|
&& (rawData[firstSTXinRawData] != DleEncoder::STX)) {
|
||||||
|
//Serial.println(rawData[firstSTXinRawData]);
|
||||||
|
firstSTXinRawData++;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (rawData[firstSTXinRawData] != DleEncoder::STX) {
|
||||||
|
//there is no STX in our data, throw it away...
|
||||||
|
//Serial.println("NO STX");
|
||||||
|
ringBuffer.deleteData(rawDataSize);
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint8_t packet[MAX_PACKET_LENGTH];
|
||||||
|
size_t packetLen;
|
||||||
|
|
||||||
|
size_t readSize;
|
||||||
|
|
||||||
|
ReturnValue_t result = DleEncoder::decode(rawData + firstSTXinRawData,
|
||||||
|
rawDataSize - firstSTXinRawData, &readSize, packet, sizeof(packet),
|
||||||
|
&packetLen);
|
||||||
|
|
||||||
|
size_t toDelete = firstSTXinRawData;
|
||||||
|
if (result == HasReturnvaluesIF::RETURN_OK) {
|
||||||
|
handlePacket(packet, packetLen);
|
||||||
|
|
||||||
|
//after handling the packet, we can delete it from the raw stream, it has been copied to packet
|
||||||
|
toDelete += readSize;
|
||||||
|
}
|
||||||
|
|
||||||
|
//remove Data which was processed
|
||||||
|
ringBuffer.deleteData(toDelete);
|
||||||
|
|
||||||
|
}
|
||||||
|
|
||||||
|
//TODO check if this is thread safe by arduino
|
||||||
|
void serialEvent() {
|
||||||
|
//Serial.println(ringBuffer.availableWriteSpace());
|
||||||
|
uint8_t i = 0;
|
||||||
|
|
||||||
|
while (Serial.available()>0) {
|
||||||
|
uint8_t byte = Serial.read();
|
||||||
|
ringBuffer.writeData(&byte, 1);
|
||||||
|
}
|
||||||
|
//Serial.println(ringBuffer.availableWriteSpace());
|
||||||
|
}
|
||||||
|
|
||||||
|
void setup() {
|
||||||
|
//CS_DDR = 0xff;
|
||||||
|
//CS_PORT = 0xff;
|
||||||
|
|
||||||
|
//CS_DDR |= (1<<);
|
||||||
|
//CS_PORT |= (1<<PB2);
|
||||||
|
Serial.begin(9600);
|
||||||
|
pinMode(MOSI, OUTPUT);
|
||||||
|
pinMode(SCK, OUTPUT);
|
||||||
|
pinMode(MISO, INPUT);
|
||||||
|
pinMode(10, OUTPUT);
|
||||||
|
digitalWrite(10,HIGH);
|
||||||
}
|
}
|
||||||
|
|
||||||
void loop() {
|
void loop() {
|
||||||
timer.tick();
|
;
|
||||||
timer.every(RING_BUFFER_CHECK_INTVL, periodicHandler1);
|
handleNewData();
|
||||||
//delay(1000);
|
delay(1000);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
||||||
|
5
makefile
Normal file
5
makefile
Normal file
@ -0,0 +1,5 @@
|
|||||||
|
TARGET = ArduinoIO
|
||||||
|
SRC = # the C source files
|
||||||
|
PSRC = main.cpp $(wildcard helper/*.cpp) arduino_core/ArduinoCore-avr/libraries/SPI/src/SPI.cpp # the C++ source files
|
||||||
|
ARDLIBS = # extra Arduino libraries
|
||||||
|
include arduino_core/arduino-base.mk # this line includes the Makefile.base
|
Reference in New Issue
Block a user