diff --git a/config/miosix_settings.h b/config/miosix_settings.h
index 0bebe9aca1212dc2c41d4f6651f7c1f3c88cc32b..cbff3deff95cc2f31d788e98048ab2f821f41b88 100644
--- a/config/miosix_settings.h
+++ b/config/miosix_settings.h
@@ -85,12 +85,12 @@ namespace miosix {
/// \def WITH_FILESYSTEM
/// Allows to enable/disable filesystem support to save code size
/// By default it is defined (filesystem support is enabled)
-//#define WITH_FILESYSTEM
+#define WITH_FILESYSTEM
/// \def WITH_DEVFS
/// Allows to enable/disable DevFs support to save code size
/// By default it is defined (DevFs is enabled)
-//#define WITH_DEVFS
+#define WITH_DEVFS
/// \def SYNC_AFTER_WRITE
/// Increases filesystem write robustness. After each write operation the
@@ -130,7 +130,7 @@ const unsigned char MAX_OPEN_FILES=8;
/// \def WITH_BOOTLOG
/// Uncomment to print bootlogs on stdout.
/// By default it is defined (bootlogs are printed)
-//#define WITH_BOOTLOG
+#define WITH_BOOTLOG
/// \def WITH_ERRLOG
/// Uncomment for debug information on stdout.
diff --git a/sbs.conf b/sbs.conf
index a04e677d3c998b5b2d966d2b60900bd82bd08360..13f75518d260dd3f8dc06e3ba195d7f508080cce 100644
--- a/sbs.conf
+++ b/sbs.conf
@@ -64,6 +64,10 @@ Files: src/shared/drivers/canbus/CanManager.cpp
src/shared/drivers/canbus/CanSocket.cpp
src/shared/drivers/canbus/CanInterrupt.cpp
+[piksi]
+Type: srcfiles
+Files: src/shared/drivers/piksi/piksi.cpp
+
# Common files (like event scheduler)
[shared]
Type: srcfiles
@@ -181,3 +185,11 @@ BinName: test-busfault
Include: %shared %i2c %math %anakin-data
Defines:
Main: test-busfault
+
+[test-piksi]
+Type: board
+BoardId: stm32f429zi_skyward_anakin
+BinName: test-piksi
+Include: %piksi
+Defines:
+Main: test-piksi
diff --git a/src/entrypoints/test-piksi.cpp b/src/entrypoints/test-piksi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7e6f2e0bf324ad17e9d14172efc72070afe5e930
--- /dev/null
+++ b/src/entrypoints/test-piksi.cpp
@@ -0,0 +1,35 @@
+#include <drivers/piksi/piksi.h>
+#include <time.h>
+#include <iostream>
+
+using namespace std;
+
+#ifdef _MIOSIX
+
+#include <miosix.h>
+
+using namespace miosix;
+
+#endif //_MIOSIX
+
+int main(int argc, char *argv[])
+{
+ Piksi piksi("/dev/gps");
+ for(;;)
+ {
+ auto gps=piksi.waitForGpsData();
+ #ifdef _MIOSIX
+ long long now=getTick();
+ #else //_MIOSIX
+ long long now=clock()/(CLOCKS_PER_SEC/1000);
+ #endif //_MIOSIX
+ cout<<" t: "<<now-gps.timestamp
+ <<" lat: "<<gps.latitude
+ <<" lon: "<<gps.longitude
+ <<" h: "<<gps.height
+ <<" vn: "<<gps.velocityNorth
+ <<" ve: "<<gps.velocityEast
+ <<" vd: "<<gps.velocityDown
+ <<" ns: "<<gps.numSatellites<<endl;
+ }
+}
\ No newline at end of file
diff --git a/src/shared/drivers/piksi/contiguous_queue.h b/src/shared/drivers/piksi/contiguous_queue.h
new file mode 100644
index 0000000000000000000000000000000000000000..51c7ef1a9ea1c2e29575bb8f3918942d11888214
--- /dev/null
+++ b/src/shared/drivers/piksi/contiguous_queue.h
@@ -0,0 +1,110 @@
+
+#ifndef CONTIGUOUS_QUEUE
+#define CONTIGUOUS_QUEUE
+
+#include <stdexcept>
+
+/**
+ * A fixed size FIFO queue whose elements are stored in a contiguous array,
+ * which allows direct access to the array in order to add/remove multiple
+ * elements.
+ *
+ * NOTE: This queue is most efficent if the data is removed form the queue in
+ * large chunks. Removing one element at a time is most inefficient since
+ * shifting all the other elements in the queue is needed. There are no
+ * performance constraints on inserting.
+ *
+ * The queue is not synchronized.
+ *
+ * \param T type of the element
+ * \param N maximum number of elements (fixed size queue)
+ */
+template<typename T, unsigned N>
+class ContiguousQueue
+{
+public:
+ /**
+ * Constructor
+ */
+ ContiguousQueue() {}
+
+ /**
+ * \return a pointer to the side of the queue where elements can be added.
+ * Call availableToAdd() to know how much space is left. The pointer is
+ * meant to be used as an array, so as to add more than one item.
+ *
+ * NOTE: if availableToAdd() returns 0 the pointer is past the end of the
+ * array and should not be dereferenced
+ */
+ T* addEnd() { return elements+size; }
+
+ /**
+ * \return a pointer to the side of the queue where elements can be removed.
+ * Call availableToRemove() to know how many elements are available. The
+ * pointer is meant to be used as an array, so as to read more than one item.
+ *
+ * NOTE: if availableToRemove() returns 0 the pointer should not be
+ * dereferenced
+ */
+ T* removeEnd() { return elements; }
+
+ /**
+ * \return a pointer to the side of the queue where elements can be removed.
+ * Call availableToRemove() to know how many elements are available. The
+ * pointer is meant to be used as an array, so as to read more than one item.
+ *
+ * NOTE: if availableToRemove() returns 0 the pointer should not be
+ * dereferenced
+ */
+ const T* removeEnd() const { return elements; }
+
+ /**
+ * \return how much space is available to add elements
+ */
+ unsigned int availableToAdd() const { return N-size; }
+
+ /**
+ * \return how many items are available to read
+ */
+ unsigned int availableToRemove() const { return size; }
+
+ /**
+ * After having added elements to the pointer obtained through addEnd(),
+ * call this function to let the queue know how many elements have been
+ * added
+ * \param n elements added
+ * \throws range_error if too many elements were addded. Memory corruption
+ * has already occurred at this point, though
+ */
+ void added(unsigned int n)
+ {
+ if(size+n>N) throw std::range_error("ContiguousBuffer::added");
+ size+=n;
+ }
+
+ /**
+ * After having removed elements to the pointer obtained through
+ * removeEnd(), call this function to let the queue know how many elements
+ * have been added removed
+ * \param n elements removed
+ * \throws range_error if too many elements were removed. Memory corruption
+ * has already occurred at this point, though
+ */
+ void removed(unsigned int n)
+ {
+ if(n>size) throw std::range_error("ContiguousBuffer::removed");
+ if(n==0) return;
+ for(unsigned int i=0;i<size-n;i++)
+ elements[i]=std::move(elements[i+n]);
+ size-=n;
+ }
+
+private:
+ ContiguousQueue(const ContiguousQueue&)=delete;
+ ContiguousQueue& operator=(const ContiguousQueue&)=delete;
+
+ T elements[N];
+ unsigned int size=0;
+};
+
+#endif //CONTIGUOUS_QUEUE
diff --git a/src/shared/drivers/piksi/piksi.cpp b/src/shared/drivers/piksi/piksi.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..680a6e1849381838350b97eea967320947d638fe
--- /dev/null
+++ b/src/shared/drivers/piksi/piksi.cpp
@@ -0,0 +1,224 @@
+
+#include "piksi.h"
+#include <unistd.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <time.h>
+#include <algorithm>
+
+using namespace std;
+
+#ifdef _MIOSIX
+
+#include <miosix.h>
+
+using namespace miosix;
+
+#endif //_MIOSIX
+
+/*
+ * Contrary to the standard CCITT CRC that starts from 0xffff, the Piksi
+ * people decided to start from 0. So we need a special CRC16 just for them
+ */
+
+static inline void crc16piksiUpdate(unsigned short& crc, unsigned char data)
+{
+ unsigned short x=((crc>>8) ^ data) & 0xff;
+ x^=x>>4;
+ crc=(crc<<8) ^ (x<<12) ^ (x<<5) ^ x;
+}
+
+static unsigned short crc16piksi(const void *message, unsigned int length)
+{
+ const unsigned char *m=reinterpret_cast<const unsigned char*>(message);
+ unsigned short result=0;
+ for(unsigned int i=0;i<length;i++) crc16piksiUpdate(result,m[i]);
+ return result;
+}
+
+//
+// class Piksi
+//
+
+Piksi::Piksi(const char *serialPath)
+{
+ fd=open(serialPath,O_RDWR);
+ if(fd<0) throw runtime_error(string("Cannot open ")+serialPath);
+ if(isatty(fd))
+ {
+ termios t;
+ tcgetattr(fd,&t);
+ t.c_lflag &= ~(ISIG | ICANON | ECHO);
+ #ifndef _MIOSIX
+ cfsetospeed(&t,B115200);
+ cfsetispeed(&t,B115200);
+ #endif //_MIOSIX
+ tcsetattr(fd,TCSANOW,&t);
+ }
+ pthread_create(&thread,NULL,&threadLauncher,this);
+ pthread_mutex_init(&mutex,NULL);
+ pthread_cond_init(&cond,NULL);
+}
+
+GPSData Piksi::getGpsData()
+{
+ GPSData result;
+ pthread_mutex_lock(&mutex);
+ if(!firstFixReceived)
+ {
+ pthread_mutex_unlock(&mutex);
+ throw runtime_error("No fix");
+ }
+ result=data;
+ pthread_mutex_unlock(&mutex);
+ return result;
+}
+
+GPSData Piksi::waitForGpsData()
+{
+ GPSData result;
+ pthread_mutex_lock(&mutex);
+ pthread_cond_wait(&cond,&mutex);
+ result=data;
+ pthread_mutex_unlock(&mutex);
+ return result;
+}
+
+Piksi::~Piksi()
+{
+ close(fd);
+ pthread_mutex_destroy(&mutex);
+ pthread_cond_destroy(&cond);
+ quit=true;
+ pthread_join(thread,NULL);
+}
+
+void* Piksi::threadLauncher(void* arg)
+{
+ reinterpret_cast<Piksi*>(arg)->run();
+ return nullptr;
+}
+
+void Piksi::run()
+{
+ do {
+ bytes.added(readData(bytes.addEnd(),bytes.availableToAdd()));
+ bytes.removed(lookForMessages(bytes.removeEnd(),bytes.availableToRemove()));
+ } while(quit==false);
+}
+
+unsigned int Piksi::readData(unsigned char *buffer, unsigned int size)
+{
+ for(;;)
+ {
+ int result=read(fd,buffer,size);
+ if(result>0) return result;
+ usleep(10000); //We want to retry but avoid 100% CPU utilization
+ return 0; //To go to a loop of run() and notice the quit flag
+ }
+}
+
+unsigned int Piksi::lookForMessages(uint8_t *buffer, unsigned int size)
+{
+// puts("###");
+// for(unsigned int i=0;i<size;i++) printf("%02x ",buffer[i]);
+// puts("");
+
+ const uint8_t preamble=0x55;
+ unsigned int consumed=0;
+ auto consume=[&](unsigned int n)
+ {
+ consumed+=n;
+ buffer+=n;
+ size-=n;
+ };
+ for(;;)
+ {
+ uint8_t *index=find(buffer,buffer+size,preamble);
+ consume(index-buffer); //Consume eventual characters between messages
+
+ if(size<sizeof(Header)) return consumed; //We don't have the header
+ auto header=reinterpret_cast<Header*>(buffer);
+ unsigned int messageSize=sizeof(Header)+header->length+crcSize;
+ if(messageSize>size) return consumed; //We don't have the entire message
+
+ uint16_t crc=*reinterpret_cast<uint16_t*>(buffer+messageSize-crcSize);
+ if(crc16piksi(buffer+1,messageSize-crcSize-1)==crc)
+ {
+ processValidMessage(buffer,messageSize);
+ consume(messageSize);
+ } else {
+ //TODO: fault counter?
+ consume(1); //Consume the preamble of the invalid message
+ }
+ }
+}
+
+void Piksi::processValidMessage(uint8_t *buffer, unsigned int size)
+{
+ Header *header=reinterpret_cast<Header*>(buffer);
+ switch(header->type)
+ {
+ case MSG_POS_LLH:
+ if(size<sizeof(MsgPosLlh)) /* TODO: fault counter? */;
+ else processPosLlh(reinterpret_cast<MsgPosLlh*>(buffer));
+ break;
+ case MSG_VEL_NED:
+ if(size<sizeof(MsgVelNed)) /* TODO: fault counter? */;
+ else processVelNed(reinterpret_cast<MsgVelNed*>(buffer));
+ break;
+ default:
+ //A valid message we're not interested in
+ break;
+ }
+}
+
+void Piksi::processPosLlh(Piksi::MsgPosLlh* msg)
+{
+ partialData.latitude=msg->lat;
+ partialData.longitude=msg->lon;
+ partialData.height=msg->height;
+ partialData.numSatellites=msg->n_sats;
+ #ifdef _MIOSIX
+ partialData.timestamp=getTick();
+ #else //_MIOSIX
+ partialData.timestamp=clock()/(CLOCKS_PER_SEC/1000);
+ #endif //_MIOSIX
+
+ if(vel && gpsTimestamp==msg->ms)
+ {
+ vel=pos=false;
+
+ pthread_mutex_lock(&mutex);
+ data=partialData;
+ firstFixReceived=true;
+ pthread_cond_broadcast(&cond);
+ pthread_mutex_unlock(&mutex);
+ } else {
+ pos=true;
+ gpsTimestamp=msg->ms;
+ }
+}
+
+void Piksi::processVelNed(Piksi::MsgVelNed* msg)
+{
+ partialData.velocityNorth=static_cast<float>(msg->n)/1000.f;
+ partialData.velocityEast=static_cast<float>(msg->e)/1000.f;
+ partialData.velocityDown=static_cast<float>(msg->d)/1000.f;
+
+ if(pos && gpsTimestamp==msg->ms)
+ {
+ vel=pos=false;
+
+ pthread_mutex_lock(&mutex);
+ data=partialData;
+ firstFixReceived=true;
+ pthread_cond_broadcast(&cond);
+ pthread_mutex_unlock(&mutex);
+ } else {
+ vel=true;
+ gpsTimestamp=msg->ms;
+ }
+}
diff --git a/src/shared/drivers/piksi/piksi.h b/src/shared/drivers/piksi/piksi.h
new file mode 100644
index 0000000000000000000000000000000000000000..51c7aae8f6350ce598194b7bceedf2b117b6daca
--- /dev/null
+++ b/src/shared/drivers/piksi/piksi.h
@@ -0,0 +1,172 @@
+
+#ifndef PIKSI_H
+#define PIKSI_H
+
+#include <pthread.h>
+#include "contiguous_queue.h"
+
+/**
+ * The GPS information
+ */
+struct GPSData
+{
+ /// timestamp in ms (anakin time, not GPS time). getTick()-timestamp tells
+ /// you how "old" the data is.
+ long long timestamp;
+
+ double latitude; ///< [deg] //TODO: cast to float??
+ double longitude; ///< [deg] //TODO: cast to float??
+ double height; ///< [m] //TODO: cast to float??
+ float velocityNorth; ///< [m/s]
+ float velocityEast; ///< [m/s]
+ float velocityDown; ///< [m/s]
+ int numSatellites; ///< [1]
+};
+
+/**
+ * Class to access the Piksi GPS.
+ *
+ * Should be connected to the Piksi UARTB configured as
+ * MODE SBP
+ * SBP message mask 65280
+ * telemetry radio on boot False
+ * baudrate 115200
+ */
+class Piksi
+{
+public:
+ /**
+ * Constructor
+ * \param serialPath path to the device file of the Piksi serial port
+ */
+ Piksi(const char *serialPath);
+
+ /**
+ * \return the latest GPS data, or throws if the GPS has not yet got a fix.
+ * If the GPS has lost the fix, the same data is returened repeatedly,
+ * use the timestamp field of the GPSData struct to know this.
+ * \throws runtime_error is no data is available
+ */
+ GPSData getGpsData();
+
+ /**
+ * \return the latest GPS data. If the GPS has yet got a fix or has lost
+ * the fix, this function will block until the fix is regained
+ */
+ GPSData waitForGpsData();
+
+ /**
+ * Destructor
+ */
+ ~Piksi();
+
+private:
+
+ Piksi(const Piksi&)=delete;
+ Piksi& operator=(const Piksi&)=delete;
+
+ struct __attribute__((packed)) Header
+ {
+ uint8_t preamble;
+ uint16_t type;
+ uint16_t sender;
+ uint8_t length;
+ };
+
+ static const unsigned int crcSize=2;
+
+ static const uint16_t MSG_POS_LLH=0x0201;
+
+ struct __attribute__((packed)) MsgPosLlh
+ {
+ Header header;
+ uint32_t ms; // [ms]
+ double lat; // [deg]
+ double lon; // [deg]
+ double height; // [m]
+ uint16_t h_accuracy; // Piksi says unimplemented
+ uint16_t v_accuracy; // Piksi says unimplemented
+ uint8_t n_sats;
+ uint8_t flags;
+ };
+
+ static const uint16_t MSG_VEL_NED=0x0205;
+
+ struct __attribute__((packed)) MsgVelNed
+ {
+ Header header;
+ uint32_t ms; // [ms]
+ int32_t n; // [mm/s]
+ int32_t e; // [mm/s]
+ int32_t d; // [mm/s]
+ uint16_t h_accuracy; // Piksi says unimplemented
+ uint16_t v_accuracy; // Piksi says unimplemented
+ uint8_t n_sats;
+ uint8_t flags;
+ };
+
+ /**
+ * Launches run() from the background thread
+ * \param arg this
+ */
+ static void *threadLauncher(void *arg);
+
+ /**
+ * Piksi main processing loop
+ */
+ void run();
+
+ /**
+ * Fill a buffer from the serial port where the piksi is connected
+ * \param buffer where to store read data
+ * \param size how many bytes to read
+ */
+ unsigned int readData(unsigned char *buffer, unsigned int size);
+
+ /**
+ * Tries to find one or more piksi message in the buffer. There are no
+ * alignment requirements, the given buffer can begin and end in the middle
+ * of a packet.
+ * \param buffer buffer read from the serial port
+ * \param size buffer size
+ * \return the number of consumed characters. the last size-consumed bytes
+ * of the buffer are not processed yet, most likely because they contain an
+ * incomplete message, and must be prepended to the buffer given at the
+ * next call in order not to miss some packets.
+ */
+ unsigned int lookForMessages(uint8_t *buffer, unsigned int size);
+
+ /**
+ * Called on a message that has already passed the CRC check.
+ * \param buffer pointer to the first message byte (0x55)
+ * \param size message size
+ */
+ void processValidMessage(uint8_t *buffer, unsigned int size);
+
+ /**
+ * Processes a POS_LLH message
+ * \param msg the message
+ */
+ void processPosLlh(MsgPosLlh* msg);
+
+ /**
+ * Processes a VEL_NED message
+ * \param msg the message
+ */
+ void processVelNed(MsgVelNed* msg);
+
+ //The queue should be large enough to contain the largest message (256+8)
+ ContiguousQueue<uint8_t,384> bytes;
+ int fd;
+ pthread_t thread;
+ pthread_mutex_t mutex;
+ pthread_cond_t cond;
+ GPSData data, partialData;
+ uint32_t gpsTimestamp=0;
+ bool pos=false;
+ bool vel=false;
+ bool firstFixReceived=false;
+ volatile bool quit=false;
+};
+
+#endif //PIKSI_H