//! Test function to validate that the fifo works as expected.
//! It is always a good idea to validate your data structures. Languages like
//! Python or Perl have such validation built in, while C++ does not. This function
//! can be used in a special build to run through some test cases and make sure
//! that the fifo behaves as we exect, especially in the edge cases, such as when
//! it gets full, wraps around, etc.
void FifoTest(HardwareSerial& ds)
{
Fifo f1(20);
Fifo::FifoType x;
char buffer[128];
for (x=1;x<25;x++) {
f1.push(&x);
sprintf(buffer,"Fifo push %d count %d\n",x,f1.count());
ds.print(buffer);
}
f1.clear();
sprintf(buffer,"Fifo count %d\n",f1.count());
ds.print(buffer);
for (x=0;x<10;x++) {
f1.push(&x);
}
sprintf(buffer,"Fifo count %d. Expected 10\n",f1.count());
ds.print(buffer);
Fifo::FifoType y;
for (x=1;x<15;x++) {
f1.pop(&y);
sprintf(buffer,"Fifo pop %d count %d\n",y,f1.count());
ds.print(buffer);
}
}
/***
* Read [pin1] [pin2] ...
*
* Read pin values
* Pins are given in the following way: A0 A1 ... for analog pins
* D0 D1 ... for digital pins
* Answer is: val1 val2 ...
*/
void cmdRead(int argC, char **argV) {
char pinType[2];
int pin;
int value;
for (int i = 1; i <= argC; i++) {
pinType[0] = argV[i][0];
pinType[1] = NULL;
pin = strtol(&(argV[i][1]), NULL, 10);
if (strcasecmp(pinType, "D") == 0) {
value = digitalRead(pin);
} else if (strcasecmp(pinType, "A") == 0) {
value = analogRead(pin);
} else {
return;
}
// Add read values to answer string
Serial.print(value);
if (i < argC) {
Serial.print(' ');
}
}
}
void setup()
{
// Set up serial port for debugging
Serial.begin(9600);
while (!Serial)
{
; // wait for serial port to connect. Needed for Leonardo only
}
// Fullfil the data array
createDataArray();
// Read and set the key from the EEPROM
readKey(KEYLENGTH);
// Inicialization of the key
time = micros(); // time start
aes192_init(key, &key_init);
emit = micros(); // time start
Serial.print(F("Inicialisation total takes: "));
Serial.print(emit - time);
Serial.println(F(" [us]"));
// initialize the digital pin as an output.
// Pin 13 has an LED connected on most Arduino boards:
pinMode(ledPin, OUTPUT);
}
void loop()
{
sendMsg = "";
recvMsg = "";
while (Serial.available()) {
sendMsg += (char)Serial.read();
delay(2);
}
if(sendMsg.length() > 0)
{
mySerial1.println(sendMsg);
Serial.print("I send: ");
Serial.println(sendMsg);
}
while (mySerial2.available()) {
recvMsg += (char)mySerial2.read();
delay(2);
}
if(recvMsg.length() > 0)
{
Serial.print("I recv: ");
Serial.println(recvMsg);
}
//delay(20);
}
File create_gps_file() {
int year;
unsigned long age;
byte month, day, hour, minute, second, hundredth;
gps.crack_datetime(&year, &month, &day, &hour, &minute, &second, &hundredth, &age);
String filename;
filename.concat(GPS_DIR);
filename.concat('/');
filename.concat(year);
filename.concat('/');
filename.concat(month);
filename.concat('/');
filename.concat(day);
filename.concat('/');
filename.concat(hour);
filename.concat(minute);
filename.concat(second);
filename.concat(".GPS");
char buf[filename.length() + 1];
filename.toCharArray(buf, sizeof(buf));
Serial.print("Criando arquivo... ");
Serial.println(buf);
return create_file(buf);
}
void loop2()
{
vw_wait_rx();
if (vw_get_message(buf, &buflen)) // Non-blocking
{
int port=parse();
switchState(port);
Serial.print(port);
Serial.println();
}
}
void Node::ajuste_pinos(){
if (sizeof(this->comandos) > 0 ){
if(this->_debug) Serial.println("Ajustando pinagem: ");
for (int i = 0; i < sizeof(this->comandos)-1; i++){
pinMode(this->comandos[i].pino, OUTPUT);
if(this->_debug) Serial.print(this->comandos[i].pino+" ");
};
if(this->_debug) Serial.println("Fim de Ajuste.");
}
}
int calcDeltaT(){
lastLoopUsefulTime = millis()-loopStartTime;
if(lastLoopUsefulTime<STD_LOOP_TIME) {
delay(STD_LOOP_TIME-lastLoopUsefulTime);
if (debugSerial && debugLoopTime){
Serial.print(" Esperei: ");
Serial.println(STD_LOOP_TIME-lastLoopUsefulTime);
}
}
lastLoopTime = millis() - loopStartTime;
loopStartTime = millis();
return lastLoopTime;
}
void debug(const char* format, ...) {
#ifdef __DEBUG__
va_list args;
va_start(args, format);
char buffer[MAX_LOG_LINE_LENGTH];
vsnprintf(buffer, MAX_LOG_LINE_LENGTH, format, args);
Serial2.print(buffer);
va_end(args);
#endif // __DEBUG__
}
void loop()
{
if (Dennao.available() > 0) {
read_buf_len = Dennao.recv(read_buf, DENNAO_RX_SIZE, 0);
for(int i=0;i<read_buf_len;i++){
Uart.print((char)read_buf[i]);
}
}
if (Uart.available() > 0) {
uint8_t incomingByte = Uart.read();
if(incomingByte=='\r'){
Dennao.send(out_buf, out_buf_len, 0);
}else{
out_buf[out_buf_len++] = incomingByte;
}
}
}
void loop(){
// update incoming values
g_ServoIn.update();
Serial.print(g_values[0]);
Serial.print(" - ");
Serial.print(g_values[1]);
Serial.print(" - ");
Serial.print(g_values[2]);
Serial.print(" - ");
Serial.println(g_values[3]);
// handle servo values here, stored in g_values
}
void Comm::process_serial()
{
if (! Serial.available()) {
return;
}
byte addr = serial_get();
byte pkt = serial_get();
byte num = serial_get();
bool okay = false;
if (!addr) { // handle by master
if (m_handler) {
okay = m_handler(num);
serial_drain();
}
else {
okay = drain(num);
}
}
else { // send to slave
okay = transmit(addr,num);
serial_drain();
}
if (okay) {
Serial.print("OK");
}
else {
Serial.print("NO");
}
Serial.print(pkt);
Serial.print(addr);
Serial.print(num);
Serial.print('\0');
}
void atualizaSensors(int deltaT){
accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
/* Calcula os angulos (em graus) de X e Y */
accYangle = (atan2(ax,az)+PI)*RAD_TO_DEG;
accXangle = (atan2(ay,az)+PI)*RAD_TO_DEG;
double gyroXrate = (double)gx/131.0;
double gyroYrate = -((double)gy/131.0);
/* Calcula os angulos X e Y do Giroscopio*/
gyroXangle += gyroXrate*((double)deltaT/1000);
gyroYangle += gyroYrate*((double)deltaT/1000);
/* Aplica o Filtro Complementar*/
compAngleX = (0.93*(compAngleX+(gyroXrate*(double)deltaT/1000)))+(0.07*accXangle);
compAngleY = (0.93*(compAngleY+(gyroYrate*(double)deltaT/1000)))+(0.07*accYangle);
// DEBUG //
if (debugSerial && debugMPU6050){
Serial.print(accXangle);
Serial.print("\t");
Serial.print(accYangle);
Serial.print("\t");
Serial.print(gyroXangle);
Serial.print("\t");
Serial.print(gyroYangle);
Serial.print("\t");
Serial.print(compAngleX);
Serial.print("\t");
Serial.print(compAngleY);
Serial.print("\t");
}
delay(1);
}
void Radio::debug(HardwareSerial& serial) {
serial.print((int)m_pressedButton);
serial.print("\t");
serial.print((int)m_bytesToSent);
}
void Radio::debugHeader(HardwareSerial& serial) {
serial.print(F("m_pressedButtonDebug\tm_bytesToSent"));
}
void printArray(const char * msg, const float * array, size_t n, HardwareSerial & serial)
{
serial.print(msg);
for (size_t i=0;i<n;i++) serial.print(array[i],8), serial.print(" ");
serial.println();
}
int FreeBoardModel::writeSimple(HardwareSerial ser) {
//ArduIMU output format
//!!!VER:1.9,RLL:-0.52,PCH:0.06,YAW:80.24,IMUH:253,MGX:44,MGY:-254,MGZ:-257,MGH:80.11,LAT:-412937350,LON:1732472000,ALT:14,COG:116,SOG:0,FIX:1,SAT:5,TOW:22504700,
ser.print("!!!VER:1.9,");
ser.print("UID:MEGA,APX:");
ser.print(autopilotState.autopilotOn);
ser.print(",APS:");
ser.print(autopilotState.autopilotReference);
//if autopilot on, send autopilot data
if (autopilotState.autopilotOn) {
ser.print(",APT:");
ser.print(getAutopilotTargetHeading());
ser.print(",APC:");
ser.print(getAutopilotCurrentHeading());
ser.print(",APR:");
ser.print(autopilotState.autopilotRudderCommand-33.0);// 0-66 in model
}
//if anchor alarm on, send data
ser.print(",AAX:");
ser.print(config.anchorAlarmOn);
ser.print(",AAR:");
ser.print(config.anchorRadius);
if (config.anchorAlarmOn) {
ser.print(",AAN:");
ser.print(config.anchorLat);
ser.print(",AAE:");
ser.print(config.anchorLon);
ser.print(",AAD:");
ser.print(getAnchorDistance());
}
//if wind alarm on, send data
ser.print(",WSX:");
ser.print(config.windAlarmOn);
ser.print(",WSK:");
ser.print(config.windAlarmSpeed);
ser.println(",");
return 0;
}
/*
* Write out the config to serial
*/
int FreeboardModel::writeConfig(HardwareSerial& ser) {
//ArduIMU output format
//!!VER:1.9,RLL:-0.52,PCH:0.06,YAW:80.24,IMUH:253,MGX:44,MGY:-254,MGZ:-257,MGH:80.11,LAT:-412937350,LON:1732472000,ALT:14,COG:116,SOG:0,FIX:1,SAT:5,TOW:22504700,
ser.print(F("!!VER:1.9,"));
ser.print(F("UID:MEGA,APX:"));
ser.print(F(",WZJ:"));
ser.print(getWindZeroOffset());
ser.print(F(",GPS:"));
ser.print(getGpsModel());
ser.print(F(",SB0:"));
ser.print(getSerialBaud());
ser.print(F(",SB1:"));
ser.print(getSerialBaud1());
ser.print(F(",SB2:"));
ser.print(getSerialBaud2());
ser.print(F(",SB3:"));
ser.print(getSerialBaud3());
ser.print(F(",STK:"));
ser.print(getSeaTalk());
ser.print(F(",LU1:"));
ser.print(getLvl1UpperLimit());
ser.print(F(",LL1:"));
ser.print(getLvl1LowerLimit());
ser.print(F(",LU2:"));
ser.print(getLvl2UpperLimit());
ser.print(F(",LL2:"));
ser.print(getLvl2LowerLimit());
ser.print(F(",LU3:"));
ser.print(getLvl3UpperLimit());
ser.print(F(",LL3:"));
ser.print(getLvl3LowerLimit());
ser.println(F(","));
return 0;
}
void setup()
{
Uart.begin(38400);
Uart.print("\rE\rSC\r");
}
void print(HardwareSerial & serial=Serial, const char * msg="", const int & format=4) const
{
serial.print(msg);
serial.println(value,format);
}
//
// printStatus
// -----------
// Print information to the Serial port
// Used during developing and debugging.
// Call it with the Serial port as parameter:
// myWire.printStatus(Serial);
// This function is not compatible with the Wire library.
// When this function is not called, it does not use any memory.
//
void SoftwareWire::printStatus( HardwareSerial& Ser)
{
Ser.println(F("-------------------"));
Ser.println(F("SoftwareWire Status"));
Ser.println(F("-------------------"));
Ser.print(F(" F_CPU = "));
Ser.println(F_CPU);
Ser.print(F(" sizeof(SoftwareWire) = "));
Ser.println(sizeof(SoftwareWire));
Ser.print(F(" _transmission status = "));
Ser.println(_transmission);
Ser.print(F(" _i2cdelay = "));
Ser.print(_i2cdelay);
if( _i2cdelay == 0)
Ser.print(F(" (free running)"));
Ser.println();
Ser.print(F(" _pullups = "));
Ser.print(_pullups);
if( _pullups)
Ser.print(F(" (enabled)"));
Ser.println();
Ser.print(F(" _timeout = "));
Ser.print(_timeout);
Ser.println(F(" ms"));
Ser.print(F(" SOFTWAREWIRE_BUFSIZE = "));
Ser.println(SOFTWAREWIRE_BUFSIZE);
Ser.print(F(" rxBufPut = "));
Ser.println(rxBufPut);
Ser.print(F(" rxBufGet = "));
Ser.println(rxBufGet);
Ser.print(F(" available() = "));
Ser.println(available());
Ser.print(F(" rxBuf (hex) = "));
for(int ii=0; ii<SOFTWAREWIRE_BUFSIZE; ii++)
{
if(rxBuf[ii] < 16)
Ser.print(F("0"));
Ser.print(rxBuf[ii],HEX);
Ser.print(F(" "));
}
Ser.println();
Ser.print(F(" _sdaPin = "));
Ser.println(_sdaPin);
Ser.print(F(" _sclPin = "));
Ser.println(_sclPin);
Ser.print(F(" _sdaBitMast = 0x"));
Ser.println(_sdaBitMask, HEX);
Ser.print(F(" _sclBitMast = 0x"));
Ser.println(_sclBitMask, HEX);
Ser.print(F(" _sdaPortReg = "));
Ser.println( (uint16_t) _sdaPortReg, HEX);
Ser.print(F(" _sclPortReg = "));
Ser.println( (uint16_t) _sclPortReg, HEX);
Ser.print(F(" _sdaDirReg = "));
Ser.println( (uint16_t) _sdaDirReg, HEX);
Ser.print(F(" _sclDirReg = "));
Ser.println( (uint16_t) _sclDirReg, HEX);
Ser.print(F(" _sdaPinReg = "));
Ser.println( (uint16_t) _sdaPinReg, HEX);
Ser.print(F(" _sclPinReg = "));
Ser.println( (uint16_t) _sclPinReg, HEX);
Ser.print(F(" line state sda = "));
Ser.println(i2c_sda_read());
Ser.print(F(" line state scl = "));
Ser.println(i2c_scl_read());
#ifdef ENABLE_I2C_SCANNER
// i2c_scanner
// Taken from : http://playground.arduino.cc/Main/I2cScanner
// At April 2015, it was version 5
Ser.println("\n I2C Scanner");
byte error, address;
int nDevices;
Ser.println(" Scanning...");
nDevices = 0;
for(address=1; address<127; address++ )
{
// The i2c_scanner uses the return value of
// the Write.endTransmisstion to see if
// a device did acknowledge to the address.
beginTransmission(address);
error = endTransmission();
if (error == 0)
{
Ser.print(" I2C device found at address 0x");
if (address<16)
Ser.print("0");
Ser.print(address,HEX);
Ser.println(" !");
nDevices++;
}
else if (error==4)
{
Ser.print(" Unknow error at address 0x");
if (address<16)
Ser.print("0");
Ser.println(address,HEX);
}
}
if (nDevices == 0)
Ser.println(" No I2C devices found\n");
else
Ser.println(" done\n");
#endif
}
void loop()
{
Serial.print(F("Before encryption of message freeMemory()= "));
Serial.print(freeRam());
Serial.println(F(" [byte]"));
time = micros(); // time start
// ------------------------------------------
// for (int piece = 0; piece < N_BLOCK; piece += N_BLOCK, pPlain += N_BLOCK)
for (int piece = 0; piece < DATALENGTH; piece += N_BLOCK, pPlain += N_BLOCK)
{
// Divide message into a block of 128 bit = 16 byte
byte cipher[N_BLOCK];
getBlockOfMsg(pPlain, cipher);
// if (beVerbose)
// {
// Serial.println();
// Serial.println("OT:");
// for (unsigned int i = 0; i < N_BLOCK; i++)
// Serial.print(char(cipher[i]));
// }
// time = micros(); // time start
aes256_enc(cipher, &key_init); // Encrypt message
// emit = micros(); // time end
// Serial.print(F("Encryption total takes: "));
// Serial.print(emit - time);
// Serial.println(F(" [ms]"));
// if (beVerbose)
// {
// Serial.println();
// Serial.println("CT:");
// for (int i = 0; i < N_BLOCK; i++)
// Serial.print(cipher[i]);
// }
// delete [] cipher; // do not forget to free ram
}
// ------------------------------------------
emit = micros(); // time start
Serial.print(F("After encryption of message freeMemory()= "));
Serial.print(freeRam());
Serial.println(F(" [byte]"));
Serial.print(F("Encryption total takes: "));
Serial.print(emit - time);
Serial.println(F(" [us]"));
Serial.println();
Serial.println(F("Light"));
digitalWrite(ledPin, HIGH); // set the LED on
delay(1000);
Serial.println(F("Dark"));
digitalWrite(ledPin, LOW); // set the LED off
delay(1000);
}
void loop()
{
Serial.print(F("Before encryption of message freeMemory()= "));
Serial.print(freeRam());
Serial.println(F(" [byte]"));
time = micros(); // time start
// ------------------------------------------
// Serial.println(F("CT:"));
for (int piece = 0; piece < DATALENGTH; piece++)
// for (int piece = 0; piece < 1; piece++)
{
// Divide message into a block of 128 bit = 16 byte
// byte* cipher = getBlockOfMsg(pPlain);
// byte vysledok = grain_getbyte(®);
// if (beVerbose)
// {
// Serial.println();
// Serial.println("OT:");
// for (unsigned int i = 0; i < N_BLOCK; i++)
// Serial.print(char(cipher[i]));
// }
// time = micros(); // time start
// present128_enc(cipher, &key_init);
plain[piece] ^= mickey128_getbyte(®);
// cipher[offset] = plainSent[offset]^vysledok;
// emit = micros(); // time end
// Serial.print(F("Encryption total takes: "));
// Serial.print(emit - time);
// Serial.println(F(" [us]"));
// if (beVerbose)
// {
// Serial.println();
// Serial.println("CT:");
// for (int i = 0; i < N_BLOCK; i++)
// Serial.print(plain[piece]);
// }
// delete [] cipher; // do not forget to free ram
}
byte hash[16];
// void hmac_md5(void* dest, void* key, uint16_t keylength_b, void* msg, uint32_t msglength_b){ /* a one-shot*/
hmac_md5(hash, key, 80, plain, 8192);
// hmac_md5(hash, key, 80, plain, 8);
// ------------------------------------------
emit = micros(); // time start
Serial.print(F("After encryption of message freeMemory()= "));
Serial.print(freeRam());
Serial.println(F(" [byte]"));
Serial.print(F("Encryption total takes: "));
Serial.print(emit - time);
Serial.println(F(" [us]"));
// Serial.println();
// Serial.println(F("OT:"));
// for (int piece = 0; piece < DATALENGTH; piece++)
// {
// plain[piece] ^= grain_getbyte(®2);
// Serial.print(char(plain[piece]));
// }
Serial.println();
Serial.println(F("Light"));
digitalWrite(ledPin, HIGH); // set the LED on
delay(1000);
Serial.println(F("Dark"));
digitalWrite(ledPin, LOW); // set the LED off
delay(1000);
}
void loop()
{
Serial.print(F("Before encryption of message freeMemory()= "));
Serial.print(freeRam());
Serial.println(F(" [byte]"));
time = micros(); // time start
// ------------------------------------------
// for (int piece = 0; piece < N_BLOCK; piece += N_BLOCK, pPlain += N_BLOCK)
for (int piece = 0; piece < DATALENGTH; piece += N_BLOCK, pPlain += N_BLOCK)
{
// Divide message into a block of 128 bit = 16 byte
// byte cipher[N_BLOCK];
// getBlockOfMsg(pPlain, cipher);
// if (beVerbose)
// {
// Serial.println();
// Serial.println("OT:");
// for (unsigned int i = 0; i < N_BLOCK; i++)
// Serial.print(char(cipher[i]));
// }
// time = micros(); // time start
// aes192_enc(cipher, &key_init); // Encrypt message
aes192_enc(pPlain, &key_init); // Encrypt message
// emit = micros(); // time end
// Serial.print(F("Encryption total takes: "));
// Serial.print(emit - time);
// Serial.println(F(" [us]"));
// if (beVerbose)
// {
// Serial.println();
// Serial.println("CT:");
// for (int i = 0; i < N_BLOCK; i++)
// Serial.print(cipher[i]);
//
// Serial.println();
// Serial.println("HASH:");
// for (int i = 0; i < N_BLOCK; i++)
// Serial.print(hash[i]);
// }
}
// void hmac_md5(void* dest, void* key, uint16_t keylength_b, void* msg, uint32_t msglength_b);
byte hash[16];
hmac_md5(hash, key, 192, plain, 8192);
// ------------------------------------------
emit = micros(); // time start
Serial.print(F("After encryption of message freeMemory()= "));
Serial.print(freeRam());
Serial.println(F(" [byte]"));
Serial.print(F("Encryption total takes: "));
Serial.print(emit - time);
Serial.println(F(" [us]"));
Serial.println();
Serial.println(F("Light"));
digitalWrite(ledPin, HIGH); // set the LED on
delay(1000);
Serial.println(F("Dark"));
digitalWrite(ledPin, LOW); // set the LED off
delay(1000);
}
void printScalar(const char * msg, const float & scalar, HardwareSerial & serial)
{
serial.print(msg);
serial.print(scalar,8);
serial.println();
}
