Skip to content
Snippets Groups Projects
Commit 0f1b3deb authored by Adrien van den Bossche's avatar Adrien van den Bossche
Browse files

First stable version of the Arduino library with example Arduino sketch

parent d8287724
Branches
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
arduino/Locapack/Locapack.cpp 0 → 100644
+ 457
− 0
View file @ 0f1b3deb
// Locapack.cpp
//
// Locapack is an Arduino library that implements the localisation packet protocol from the IRIT/RMESS team.
#include <SPI.h>
#include "Locapack.h"
#include <machine/endian.h>
///
/// @brief The protocols name enumeration as strings
///
const char* protocolsListAsString[] = { "TWR", "SDS-TWR", "BB-TWR" };
Locapack::Locapack()
{
_sequence_number = 0;
setdevice_id16(0);
}
void Locapack::setdevice_id16(uint16_t device_id)
{
encodeUint16(device_id, _device_id.device_id_buf);
_device_id.device_id_len = 2;
}
void Locapack::setdevice_id32(uint32_t device_id)
{
encodeUint32(device_id, _device_id.device_id_buf);
_device_id.device_id_len = 4;
}
void Locapack::setdevice_id64(uint64_t device_id)
{
encodeUint64(device_id, _device_id.device_id_buf);
_device_id.device_id_len = 8;
}
int Locapack::makeHeader(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
uint8_t timestamp_presence_flag, uint8_t device_id_size, packet_type_t packet_type, uint8_t* buffer)
{
buffer[1] = (uint8_t)(((protocol_version & 0x0f) << 4) | (device_id_size & 0x0f));
buffer[0] = (uint8_t)(((packet_type & 0x0f) << 4)
| ((movement_id_presence_flag & 0x01) << 3)
| ((timestamp_presence_flag & 0x01) << 2));
return 2;
}
int Locapack::decodeHeader(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
uint8_t* timestamp_presence_flag, uint8_t* device_id_size, packet_type_t* packet_type, uint8_t* buffer)
{
*protocol_version = (protocol_version_t)((buffer[1] >> 4) & 0x0f);
*device_id_size = buffer[1] & 0x0f;
*packet_type = (packet_type_t)((buffer[0] >> 4) & 0x0f);
*movement_id_presence_flag = (buffer[0] >> 3) & 0x01;
*timestamp_presence_flag = (buffer[0] >> 2) & 0x01;
return 2;
}
int Locapack::makePacket(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
uint8_t timestamp_presence_flag, packet_type_t packet_type, uint16_t sequence_number,
uint64_t timestamp, uint8_t movement_id, device_id_t* device_id, uint8_t* payload,
uint8_t payload_len, uint8_t* buffer)
{
int len = 0;
int device_id_len = 0;
if ( device_id != NULL ) device_id_len = device_id->device_id_len;
len += makeHeader(protocol_version, movement_id_presence_flag, timestamp_presence_flag,
device_id_len, packet_type, buffer);
encodeUint16(sequence_number, &buffer[len]); len += 2;
if ( timestamp_presence_flag )
{
encodeUint40(timestamp, &buffer[len]);
len += 5;
}
if ( movement_id_presence_flag )
{
buffer[len] = movement_id;
len += 1;
}
if ( device_id_len != 0 )
{
memcpy(&buffer[len], device_id->device_id_buf, device_id->device_id_len);
len += device_id->device_id_len;
}
buffer[len] = payload_len;
len += 1;
if ( payload_len > 0 )
{
memcpy(&buffer[len], payload, payload_len);
len += payload_len;
}
return len;
}
int Locapack::decodePacket(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
uint8_t* timestamp_presence_flag, packet_type_t* packet_type, uint16_t* sequence_number,
uint64_t* timestamp, uint8_t* movement_id, device_id_t* device_id, uint8_t* payload,
uint8_t* payload_len, uint8_t* buffer)
{
int len = 0;
len += decodeHeader(protocol_version, movement_id_presence_flag, timestamp_presence_flag,
&device_id->device_id_len, packet_type, buffer);
*sequence_number = decodeUint16(&buffer[len]); len += 2;
if ( *timestamp_presence_flag )
{
*timestamp = decodeUint40(&buffer[len]);
len += 5;
}
if ( *movement_id_presence_flag )
{
*movement_id = buffer[len];
len += 1;
}
if ( device_id->device_id_len > 0 )
{
memcpy(device_id->device_id_buf, &buffer[len], device_id->device_id_len);
len += device_id->device_id_len;
}
*payload_len = buffer[len];
len += 1;
if ( *payload_len > 0 )
{
memcpy(payload, &buffer[len], *payload_len);
len += *payload_len;
}
return len;
}
int Locapack::makeUniversalGnssPayload(float latitude, float longitude, bool altitude_present, float altitude,
bool dop_present, float dop, uint8_t* buffer)
{
int len = 0;
buffer[0] = (altitude_present == true ? 0x80 : 0) | (dop_present == true ? 0x40 : 0);
len += 1;
encodeFloat(latitude, &buffer[len]); len += 4;
encodeFloat(longitude, &buffer[len]); len += 4;
if (altitude_present)
{
encodeFloat(altitude, &buffer[len]); len += 4;
}
if (dop_present)
{
encodeFloat(dop, &buffer[len]); len += 4;
}
return len;
}
int Locapack::decodeUniversalGnssPayload(float* latitude, float* longitude, bool* altitude_present, float* altitude,
bool* dop_present, float* dop, uint8_t* buffer)
{
int len = 0;
if (buffer[0] & 0x80) *altitude_present = true; else *altitude_present = false;
if (buffer[0] & 0x40) *dop_present = true; else *dop_present = false;
len += 1;
*latitude = decodeFloat(&buffer[len]); len += 4;
*longitude = decodeFloat(&buffer[len]); len += 4;
if (*altitude_present)
{
*altitude = decodeFloat(&buffer[len]); len += 4;
}
if (*dop_present)
{
*dop = decodeFloat(&buffer[len]); len += 4;
}
return len;
}
int Locapack::createUniversalGnssPacket(universalGnssPacket_t* universalGnssPacket, uint8_t* buffer)
{
int len = 0;
uint8_t payload_buffer[MAX_BUFFER_LEN];
int payload_buffer_len = 0;
payload_buffer_len = makeUniversalGnssPayload(
universalGnssPacket->latitude,
universalGnssPacket->longitude,
universalGnssPacket->altitude_present,
universalGnssPacket->altitude,
universalGnssPacket->dop_present,
universalGnssPacket->dop,
payload_buffer);
// protocol_version_t PROTOCOL_V1
// uint8_t movement_id_presence_flag false
// uint8_t timestamp_presence_flag true
// packet_type_t packet_type PACKET_TYPE_UNIVERSAL_GNSS
// uint16_t sequence_number (internal sequence_number)
// uint64_t timestamp (millis)
// uint8_t movement_id disabled
// device_id_t* device_id (internal device_id)
// uint8_t* payload the UniversalGnssPacket generated
// uint8_t payload_len the generated packet length
// uint8_t* buffer the given buffer
len = makePacket(PROTOCOL_V1, false, true, PACKET_TYPE_UNIVERSAL_GNSS, ++_sequence_number,
(uint64_t)millis(), 0, &_device_id, payload_buffer, payload_buffer_len, buffer);
return len;
}
int Locapack::parseLocaPacket(locapacket_t* locapacket, uint8_t* buffer)
{
int len = 0;
int decoded_payload_len = 0;
protocol_version_t protocol_version;
uint8_t movement_id_presence_flag;
uint8_t timestamp_presence_flag;
packet_type_t packet_type;
uint16_t sequence_number;
uint64_t timestamp;
uint8_t movement_id;
device_id_t device_id;
uint8_t payload[MAX_BUFFER_LEN];
uint8_t payload_len;
len = decodePacket(&protocol_version, &movement_id_presence_flag, &timestamp_presence_flag, &packet_type,
&sequence_number, &timestamp, &movement_id, &device_id, payload, &payload_len, buffer);
if (protocol_version != PROTOCOL_V1) return 0;
if (payload_len == 0) return 0;
switch (packet_type)
{
case PACKET_TYPE_UNIVERSAL_GNSS:
decoded_payload_len = decodeUniversalGnssPayload(&locapacket->universalGnssPacket.latitude,
&locapacket->universalGnssPacket.longitude,
&locapacket->universalGnssPacket.altitude_present,
&locapacket->universalGnssPacket.altitude,
&locapacket->universalGnssPacket.dop_present,
&locapacket->universalGnssPacket.dop,
payload);
if (decoded_payload_len != payload_len) return 0;
break;
case PACKET_TYPE_LOCALLY_REFERENCED:
return 0; // ToDO
break;
default:
return 0;
}
return len;
}
uint16_t Locapack::decodeUint16(uint8_t *data)
{
return 0 | (data[1] << 8) | data[0];
}
void Locapack::encodeUint16(uint16_t from, uint8_t *to)
{
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
uint32_t Locapack::decodeUint32(uint8_t *data)
{
return 0 | (data[3] << 24) | (data[2] << 16) | (data[1] << 8) | data[0];
}
void Locapack::encodeUint32 ( uint32_t from, uint8_t *to )
{
to[3] = (from & 0xFF000000) >> 24;
to[2] = (from & 0xFF0000) >> 16;
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
uint64_t Locapack::decodeUint40 ( uint8_t *data )
{
uint64_t tmp = 0;
tmp = data[4];
tmp = tmp << 32;
tmp = tmp | decodeUint32(data);
return tmp;
}
void Locapack::encodeUint40 ( uint64_t from, uint8_t *to )
{
to[4] = (from & 0xFF00000000) >> 32;
to[3] = (from & 0xFF000000) >> 24;
to[2] = (from & 0xFF0000) >> 16;
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
uint64_t Locapack::decodeUint64 ( uint8_t *data )
{
uint64_t tmp = 0;
tmp = decodeUint32(&data[4]); // | decodeUint32(data);
tmp = tmp << 32;
tmp = tmp | decodeUint32(data);
return tmp;
}
void Locapack::encodeUint64 ( uint64_t from, uint8_t *to )
{
to[7] = (from & 0xFF00000000000000) >> 56;
to[6] = (from & 0xFF000000000000) >> 48;
to[5] = (from & 0xFF0000000000) >> 40;
to[4] = (from & 0xFF00000000) >> 32;
to[3] = (from & 0xFF000000) >> 24;
to[2] = (from & 0xFF0000) >> 16;
to[1] = (from & 0xFF00) >> 8;
to[0] = from & 0xFF;
}
float Locapack::decodeFloat ( uint8_t *data ) {
typedef union _data {
float f;
char s[4];
} myData;
myData q;
#ifndef BYTE_ORDER
#error "BYTE_ORDER must be defined"
#endif
#if BYTE_ORDER==LITTLE_ENDIAN
q.s[0] = data[0];
q.s[1] = data[1];
q.s[2] = data[2];
q.s[3] = data[3];
#else
q.s[0] = data[3];
q.s[1] = data[2];
q.s[2] = data[1];
q.s[3] = data[0];
#endif
return q.f;
}
void Locapack::encodeFloat ( float from, uint8_t *to ) {
typedef union _data {
float f;
char s[4];
} myData;
myData q;
q.f = from;
#ifndef BYTE_ORDER
#error "BYTE_ORDER must be defined"
#endif
#if BYTE_ORDER==LITTLE_ENDIAN
to[0] = q.s[0];
to[1] = q.s[1];
to[2] = q.s[2];
to[3] = q.s[3];
#else
to[0] = q.s[3];
to[1] = q.s[2];
to[2] = q.s[1];
to[3] = q.s[0];
#endif
}
double Locapack::decodeDouble ( uint8_t *data ) {
typedef union _data {
double f;
char s[8];
} myData;
myData q;
#ifndef BYTE_ORDER
#error "BYTE_ORDER must be defined"
#endif
#if BYTE_ORDER==LITTLE_ENDIAN
q.s[0] = data[0];
q.s[1] = data[1];
q.s[2] = data[2];
q.s[3] = data[3];
q.s[4] = data[4];
q.s[5] = data[5];
q.s[6] = data[6];
q.s[7] = data[7];
#else
q.s[0] = data[7];
q.s[1] = data[6];
q.s[2] = data[5];
q.s[3] = data[4];
q.s[4] = data[3];
q.s[5] = data[2];
q.s[6] = data[1];
q.s[7] = data[0];
#endif
return q.f;
}
void Locapack::encodeDouble ( double from, uint8_t *to ) {
typedef union _data {
double f;
char s[8];
} myData;
myData q;
q.f = from;
#ifndef BYTE_ORDER
#error "BYTE_ORDER must be defined"
#endif
#if BYTE_ORDER==LITTLE_ENDIAN
to[0] = q.s[0];
to[1] = q.s[1];
to[2] = q.s[2];
to[3] = q.s[3];
to[4] = q.s[4];
to[5] = q.s[5];
to[6] = q.s[6];
to[7] = q.s[7];
#else
to[0] = q.s[7];
to[1] = q.s[6];
to[2] = q.s[5];
to[3] = q.s[4];
to[4] = q.s[3];
to[5] = q.s[2];
to[6] = q.s[1];
to[7] = q.s[0];
#endif
}
arduino/Locapack/Locapack.h 0 → 100644
+ 261
− 0
View file @ 0f1b3deb
// Locapack.h
//
// Locapack is an Arduino library that implements the localisation packet protocol from the IRIT/RMESS team.
//
/// \mainpage Locapack library for Arduino
///
/// @author Adrien van den Bossche <vandenbo@univ-tlse2.fr>
#ifndef Locapack_h
#define Locapack_h
#include "Arduino.h"
class Locapack {
public:
#define MAX_DEVICE_ID_LEN 8
#define MAX_BUFFER_LEN 128
///
/// @brief
///
typedef enum {
PROTOCOL_V1 = 1
} protocol_version_t;
///
/// @brief
///
typedef enum {
PACKET_TYPE_UNIVERSAL_GNSS = 1,
PACKET_TYPE_LOCALLY_REFERENCED = 2
} packet_type_t;
///
/// @brief
///
typedef struct {
uint8_t device_id_buf[MAX_DEVICE_ID_LEN];
uint8_t device_id_len;
} device_id_t;
///
/// @brief
///
typedef struct {
float latitude;
float longitude;
bool altitude_present;
float altitude;
bool dop_present;
float dop;
} universalGnssPacket_t;
///
/// @brief
///
typedef struct {
float x;
float y;
bool z_present;
float z;
bool dop_present;
float dop;
bool frameofref_id_presence_flag;
uint64_t frameofref_id;
} locallyReferencedPacket_t;
///
/// @brief
///
typedef union locapacket {
universalGnssPacket_t universalGnssPacket;
locallyReferencedPacket_t locallyReferencedPacket;
} locapacket_t;
///
/// @brief
///
device_id_t _device_id;
///
/// @brief
///
uint16_t _sequence_number;
///
/// @brief Locapack Constructor
///
Locapack();
///
/// @brief
/// @param
///
void setdevice_id16(uint16_t sourceAddress);
///
/// @brief
/// @param
///
void setdevice_id32(uint32_t sourceAddress);
///
/// @brief
/// @param
///
void setdevice_id64(uint64_t sourceAddress);
///
/// @brief
/// @param
///
int makeHeader(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
uint8_t timestamp_presence_flag, uint8_t device_id_size, packet_type_t packet_type, uint8_t* buffer);
///
/// @brief
/// @param
///
int decodeHeader(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
uint8_t* timestamp_presence_flag, uint8_t* device_id_size, packet_type_t* packet_type, uint8_t* buffer);
///
/// @brief
/// @param
///
int makePacket(protocol_version_t protocol_version, uint8_t movement_id_presence_flag,
uint8_t timestamp_presence_flag, packet_type_t packet_type, uint16_t sequence_number,
uint64_t timestamp, uint8_t movement_id, device_id_t* device_id, uint8_t* payload,
uint8_t payload_len, uint8_t* buffer);
///
/// @brief
/// @param
///
int decodePacket(protocol_version_t* protocol_version, uint8_t* movement_id_presence_flag,
uint8_t* timestamp_presence_flag, packet_type_t* packet_type, uint16_t* sequence_number,
uint64_t* timestamp, uint8_t* movement_id, device_id_t* device_id, uint8_t* payload,
uint8_t* payload_len, uint8_t* buffer);
///
/// @brief
/// @param
///
int makeUniversalGnssPayload(float latitude, float longitude, bool altitude_present, float altitude,
bool dop_present, float dop, uint8_t* buffer);
///
/// @brief
/// @param
///
int decodeUniversalGnssPayload(float* latitude, float* longitude, bool* altitude_present, float* altitude,
bool* dop_present, float* dop, uint8_t* buffer);
///
/// @brief
/// @param
///
int createUniversalGnssPacket(universalGnssPacket_t* gnss, uint8_t* buffer);
///
/// @brief
/// @param
///
int parseLocaPacket(locapacket_t* locapacket, uint8_t* buffer);
private:
///
/// @brief Builds an uint16 value from two uint8 values
/// @param data The address of the uint8_t buffer
/// @return The decoded uint16_t
///
uint16_t decodeUint16 ( uint8_t *data );
///
/// @brief Formats an uint16 value as a list of uint8 values
/// @param from The uint16_t value
/// @param to The address of the uint8_t buffer
///
void encodeUint16 ( uint16_t from, uint8_t *to );
///
/// @brief Builds an uint32 value from four uint8 values
/// @param data The address of the uint8_t buffer
/// @return The decoded uint32_t
///
uint32_t decodeUint32 ( uint8_t *data );
///
/// @brief Formats an uint32 value as a list of uint8 values
/// @param from The uint32_t value
/// @param to The address of the uint8_t buffer
///
void encodeUint32 ( uint32_t from, uint8_t *to );
///
/// @brief Builds an uint64 value from five uint8 values
/// @param data The address of the uint8_t buffer
/// @return The decoded uint64_t
///
uint64_t decodeUint40 ( uint8_t *data );
///
/// @brief Formats an uint64 value with only 5 LSbytes as a list of uint8 values
/// @param from The uint64_t value
/// @param to The address of the uint8_t buffer
///
void encodeUint40 ( uint64_t from, uint8_t *to );
///
/// @brief Builds an uint64 value from eight uint8 values
/// @param data The address of the uint8_t buffer
/// @return The decoded uint64_t
///
uint64_t decodeUint64 ( uint8_t *data );
///
/// @brief Formats an uint64 value as a list of uint8 values
/// @param from The uint64_t value
/// @param to The address of the uint8_t buffer
///
void encodeUint64 ( uint64_t from, uint8_t *to );
///
/// @brief Builds a float value from four uint8 values
/// @param data The address of the uint8_t buffer
/// @return The decoded float
///
float decodeFloat ( uint8_t *data );
///
/// @brief Formats an float value as a list of uint8 values
/// @param from The float value
/// @param to The address of the uint8_t buffer
/// @return No return
///
void encodeFloat ( float from, uint8_t *to );
///
/// @brief Builds a double value from four uint8 values
/// @param data The address of the uint8_t buffer
/// @return The decoded double
///
double decodeDouble ( uint8_t *data );
///
/// @brief Formats an float value as a list of uint8 values
/// @param from The float value
/// @param to The address of the uint8_t buffer
/// @return No return
///
void encodeDouble ( double from, uint8_t *to );
protected:
};
#endif
arduino/Locapack/examples/tinygpsplus/tinygpsplus.ino 0 → 100644
+ 81
− 0
View file @ 0f1b3deb
#include <Locapack.h>
#include <TinyGPS++.h>
#define LOCAPACK_PACKET_PERIOD 5000
Locapack locapack;
TinyGPSPlus gps;
uint8_t buffer[128];
Locapack::device_id_t device_id;
Locapack::universalGnssPacket_t gnss;
Locapack::locapacket_t locapacket;
bool gnss_valid = false;
uint32_t gnss_age = 0;
uint32_t timetosend = LOCAPACK_PACKET_PERIOD;
void setup()
{
// Set console and GPS serial baudrate
Serial.begin(115200);
Serial1.begin(9600);
locapack.setdevice_id16(0x1234);
gnss.altitude_present = true;
gnss.dop_present = true;
}
void loop()
{
if (millis() > timetosend)
{
timetosend += LOCAPACK_PACKET_PERIOD;
if (gnss_valid)
{
Serial.print("I create a new packet with position recorded ");
Serial.print(millis()-gnss_age);
Serial.println("ms ago.");
int len = locapack.createUniversalGnssPacket(&gnss, buffer);
for (int i=0; i<len; i++)
{
Serial.print(buffer[i], HEX);
Serial.print(" ");
}
Serial.println();
Serial.println("Now decode this packet...");
len = locapack.parseLocaPacket(&locapacket, buffer);
Serial.print("packet len="); Serial.println(len);
Serial.print("latitude="); Serial.println(locapacket.universalGnssPacket.latitude);
Serial.print("longitude="); Serial.println(locapacket.universalGnssPacket.longitude);
Serial.print("altitude="); Serial.println(locapacket.universalGnssPacket.altitude);
Serial.print("hdop="); Serial.println(locapacket.universalGnssPacket.dop);
}
else
{
Serial.println("\n\n*** Waiting for a valid position ***\n\n");
}
}
// Get chars from GPS receiver
while (Serial1.available())
{
char c = Serial1.read();
//Serial.write(c); // uncomment this line if you want to see the GPS data flowing
if (gps.encode(c))
{
// Did a new valid sentence come in?
if ( gps.location.isValid() )
{
gnss.latitude = gps.location.lat();
gnss.longitude = gps.location.lng();
gnss.altitude = gps.altitude.meters();
gnss.dop = gps.hdop.hdop();
gnss_age = millis();
gnss_valid = true;
}
}
}
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment