├── .gitattributes
├── source
    ├── .vscode
    │   └── settings.json
    ├── makefile
    ├── timestamp.h
    ├── controller.h
    ├── joystick.cpp
    ├── fhl_ld19.h
    ├── example.cpp
    ├── joystick.h
    ├── maus_board.h
    ├── controller.cpp
    ├── fhl_ld19.cpp
    └── maus_board.cpp
├── ESP32_firmware
    └── main
    │   ├── KISS_telemetry_protocol.pdf
    │   ├── esc_telemetry.h
    │   ├── messaging.h
    │   └── main.ino
└── README.md


/.gitattributes:
--------------------------------------------------------------------------------
1 | # Auto detect text files and perform LF normalization
2 | * text=auto
3 | 


--------------------------------------------------------------------------------
/source/.vscode/settings.json:
--------------------------------------------------------------------------------
1 | {
2 |     "files.associations": {
3 |         "*.ino": "cpp",
4 |         "atomic": "cpp"
5 |     }
6 | }


--------------------------------------------------------------------------------
/ESP32_firmware/main/KISS_telemetry_protocol.pdf:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/JBarrada/maus_board/HEAD/ESP32_firmware/main/KISS_telemetry_protocol.pdf


--------------------------------------------------------------------------------
/source/makefile:
--------------------------------------------------------------------------------
 1 | LIBS=-lm -pthread
 2 | OPTIONS=-O2 -Wno-psabi -std=c++17
 3 | 
 4 | clean:
 5 | 	rm -f *.o 
 6 | 
 7 | %.o: %.cpp
 8 | 	g++ -c $< $(LIBS) $(OPTIONS) -o $@
 9 | 
10 | example: clean maus_board.o fhl_ld19.o joystick.o controller.o example.cpp 
11 | 	g++ example.cpp maus_board.o fhl_ld19.o joystick.o controller.o $(LIBS) $(OPTIONS) -o $@


--------------------------------------------------------------------------------
/source/timestamp.h:
--------------------------------------------------------------------------------
 1 | #ifndef __TIMESTAMP_H__
 2 | #define __TIMESTAMP_H__
 3 | 
 4 | #include <stdint.h>
 5 | #include <chrono>
 6 | 
 7 | #define NSECS_TO_SECS 1000000000
 8 | #define NSECS_TO_MSECS 1000000
 9 | #define NSECS_TO_USECS 1000
10 | 
11 | class TimeStamp {
12 | public:
13 |     // Returns a nanoseconds timestamp since epoch
14 |     static uint64_t get() {
15 |         return std::chrono::high_resolution_clock::now().time_since_epoch().count();
16 |     }
17 | };
18 | 
19 | #endif
20 | 


--------------------------------------------------------------------------------
/source/controller.h:
--------------------------------------------------------------------------------
 1 | #ifndef __CONTROLLER_H__
 2 | #define __CONTROLLER_H__
 3 | 
 4 | // Hey, whats up. This code is pretty gross. Please don't look too hard.
 5 | // If you have your own joystick handling code, please just use that. It is probably better than this. 
 6 | 
 7 | #include <sys/stat.h>
 8 | #include <atomic>
 9 | #include <thread>
10 | #include <unistd.h>
11 | 
12 | #include "joystick.h"
13 | #include "timestamp.h"
14 | 
15 | class Controller {
16 | private:
17 |     const std::string DEFAULT_JOYSTICK = "/dev/input/js0";
18 | 
19 |     // Controls how often to check if the joystick is found/connected
20 |     static const uint64_t MIN_CONNECTED_CHECK_NSECS = 5 * (uint64_t)NSECS_TO_SECS; // 5 seconds
21 |     uint64_t lastConnectedCheckTimestamp = 0;
22 | 
23 |     // Minimum left trigger (braking) amount to cancel autonomous mode
24 |     const float autonomousModeCancelLeftTriggerMinimum = 0.1f;
25 | 
26 |     // Actual joystick instance for polling
27 |     Joystick joystick;
28 | 
29 |     static const uint32_t POLLING_LOOP_DELAY_USECS = 10000;
30 |     bool isPolling = false;
31 |     std::thread pollingThread;
32 | 
33 |     float map(const float in, const float inMin, const float inMax, const float outMin, const float outMax);
34 |     bool isJoystickAvailable();
35 |     bool tryConnect();
36 |     void disconnected();
37 |     void poll();
38 |     void pollLoop();
39 | public:
40 |     std::atomic<float> rightTriggerPos;
41 |     std::atomic<float> leftTriggerPos;
42 |     std::atomic<float> throttlePos;
43 |     std::atomic<float> steeringPos;
44 | 
45 |     std::atomic<bool> autonomousModeActive;
46 |     std::atomic<bool> recordingModeActive;
47 |     std::atomic<bool> isConnected;
48 | 
49 |     std::atomic<bool> aButtonPressed;
50 |     std::atomic<bool> bButtonPressed;
51 |     std::atomic<bool> xButtonPressed;
52 |     std::atomic<bool> yButtonPressed;
53 | 
54 |     std::atomic<bool> dPadUpPressed;
55 |     std::atomic<bool> dPadDownPressed;
56 |     std::atomic<bool> dPadLeftPressed;
57 |     std::atomic<bool> dPadRightPressed;
58 | 
59 |     Controller() : isConnected(false) { disconnected(); }
60 |     ~Controller() { stopPolling(); }
61 | 
62 |     bool startPolling();
63 |     bool stopPolling();
64 | };
65 | 
66 | #endif


--------------------------------------------------------------------------------
/source/joystick.cpp:
--------------------------------------------------------------------------------
 1 | // Licensed under the Apache License, Version 2.0 (the "License");
 2 | // you may not use this file except in compliance with the License.
 3 | // You may obtain a copy of the License at
 4 | //
 5 | //   http://www.apache.org/licenses/LICENSE-2.0
 6 | //
 7 | // Unless required by applicable law or agreed to in writing, software
 8 | // distributed under the License is distributed on an "AS IS" BASIS,
 9 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
10 | // See the License for the specific language governing permissions and
11 | // limitations under the License.
12 | //
13 | // Copyright Drew Noakes 2013-2016
14 | 
15 | #include "joystick.h"
16 | 
17 | #include <sys/types.h>
18 | #include <sys/stat.h>
19 | #include <fcntl.h>
20 | #include <iostream>
21 | #include <string>
22 | #include <sstream>
23 | #include "unistd.h"
24 | 
25 | Joystick::Joystick()
26 | {
27 |   openPath("/dev/input/js0");
28 | }
29 | 
30 | Joystick::Joystick(int joystickNumber)
31 | {
32 |   std::stringstream sstm;
33 |   sstm << "/dev/input/js" << joystickNumber;
34 |   openPath(sstm.str());
35 | }
36 | 
37 | Joystick::Joystick(std::string devicePath)
38 | {
39 |   openPath(devicePath);
40 | }
41 | 
42 | Joystick::Joystick(std::string devicePath, bool blocking)
43 | {
44 |   openPath(devicePath, blocking);
45 | }
46 | 
47 | void Joystick::openPath(std::string devicePath, bool blocking)
48 | {
49 |   // Open the device using either blocking or non-blocking
50 |   _fd = open(devicePath.c_str(), blocking ? O_RDONLY : O_RDONLY | O_NONBLOCK);
51 | }
52 | 
53 | bool Joystick::sample(JoystickEvent* event)
54 | {
55 |   int bytes = read(_fd, event, sizeof(*event)); 
56 | 
57 |   if (bytes == -1)
58 |     return false;
59 | 
60 |   // NOTE if this condition is not met, we're probably out of sync and this
61 |   // Joystick instance is likely unusable
62 |   return bytes == sizeof(*event);
63 | }
64 | 
65 | bool Joystick::isFound()
66 | {
67 |   return _fd >= 0;
68 | }
69 | 
70 | Joystick::~Joystick()
71 | {
72 |   close(_fd);
73 | }
74 | 
75 | std::ostream& operator<<(std::ostream& os, const JoystickEvent& e)
76 | {
77 |   os << "type=" << static_cast<int>(e.type)
78 |      << " number=" << static_cast<int>(e.number)
79 |      << " value=" << static_cast<int>(e.value);
80 |   return os;
81 | }
82 | 
83 | 
84 | 


--------------------------------------------------------------------------------
/ESP32_firmware/main/esc_telemetry.h:
--------------------------------------------------------------------------------
 1 | #ifndef __ESC_TELEMETRY_H__
 2 | #define __ESC_TELEMETRY_H__
 3 | 
 4 | // Simple class for reading KISS ESC type telemetry messages
 5 | 
 6 | #include <stddef.h>
 7 | #include <stdint.h>
 8 | 
 9 | #define ESC_TELEMETRY_BUF_SIZE 10
10 | 
11 | class ESCTelemetry {
12 | private:
13 |     // Circular buffer for reading in bytes from the serial interface
14 |     uint8_t escTelemetryBuf[ESC_TELEMETRY_BUF_SIZE];
15 |     uint8_t escTelemetryBufPos = 0;
16 | 
17 |     // Stream to use
18 |     Stream &serialPort;
19 | 
20 | public:
21 |     // Successfully parsed telemetry packets will get memcpy'd to here
22 |     uint8_t buffer[ESC_TELEMETRY_BUF_SIZE];
23 | 
24 |     ESCTelemetry(Stream &serialPort) : serialPort(serialPort) {}
25 | 
26 |     // KISS ESC telemetry CRC calculation
27 |     uint8_t getCRC8(uint8_t *data, uint8_t length) {
28 |         uint8_t crc = 0;
29 |         for (uint8_t i = 0; i < length; i++) {
30 |             uint8_t crcU = data[i];
31 |             crcU ^= crc;
32 |             for (uint8_t j = 0; j < 8; j++)
33 |                 crcU = (crcU & 0x80) ? 0x7 ^ (crcU << 1) : (crcU << 1);
34 |             crc = crcU;
35 |         }
36 |         return crc;
37 |     }
38 | 
39 |     // Main update function. Run this as often as possible.
40 |     // Returns true if a packet was successfully parsed. Packet will get memcpy'd to the public 'buffer'
41 |     bool update() {
42 |         bool packetReceived = false;
43 | 
44 |         while (serialPort.available()) {
45 |             // Read in the next byte from the serial interface into the circular buffer
46 |             escTelemetryBuf[escTelemetryBufPos] = serialPort.read();
47 |             
48 |             // Unwrap the circular buffer into tempBuffer so that byte 0 of the telemetry packet actually starts at intex 0
49 |             uint8_t tempBuffer[ESC_TELEMETRY_BUF_SIZE];
50 |             for (uint8_t i = 0; i < ESC_TELEMETRY_BUF_SIZE; i++)
51 |               tempBuffer[i] = escTelemetryBuf[(escTelemetryBufPos + i + 1) % ESC_TELEMETRY_BUF_SIZE];
52 | 
53 |             // Check tempBuffer CRC
54 |             const uint8_t calculatedCRC = getCRC8(tempBuffer, ESC_TELEMETRY_BUF_SIZE - 1);
55 |             if (calculatedCRC == tempBuffer[ESC_TELEMETRY_BUF_SIZE - 1]) {
56 |               // Copy the private buffer to a public buffer
57 |               memcpy(buffer, tempBuffer, ESC_TELEMETRY_BUF_SIZE);
58 |               packetReceived = true;
59 |             }
60 | 
61 |             // Increment escTelemetryBufPos
62 |             escTelemetryBufPos = (escTelemetryBufPos + 1) % ESC_TELEMETRY_BUF_SIZE;            
63 |         }
64 | 
65 |         return packetReceived;
66 |     }
67 | };
68 | 
69 | #endif


--------------------------------------------------------------------------------
/source/fhl_ld19.h:
--------------------------------------------------------------------------------
 1 | #ifndef __FHL_LD19_H__
 2 | #define __FHL_LD19_H__
 3 | 
 4 | // Basic driver for FHL-LD19 lidar
 5 | // Written by Justin Barrada
 6 | 
 7 | #include <stdint.h>
 8 | #include <stdio.h>
 9 | #include <vector>
10 | #include <unistd.h>
11 | #include <fcntl.h>
12 | #include <termios.h>
13 | #include <thread>
14 | #include <string.h>
15 | 
16 | #include "timestamp.h"
17 | 
18 | #define DEFAULT_SERIAL_FHL_LD19 "/dev/serial0"
19 | 
20 | class LD19 {
21 | public:
22 |     struct __attribute__((__packed__)) LidarPoint {
23 |         uint16_t distance;  // Millimeters
24 |         uint8_t intensity;  // Docs say for an object at 6M, this value should be around 200
25 |         uint16_t angle;     // 0.01 degrees
26 |         uint64_t timestamp; // Nanoseconds since epoch
27 |     };
28 | 
29 | private:
30 |     static const uint8_t crcTable[256];
31 |     
32 |     struct __attribute__((__packed__)) RawPoint {
33 |         uint16_t distance; // Millimeters
34 |         uint8_t intensity; // Docs say for an object at 6M, this value should be around 200 
35 |     };
36 | 
37 |     static const uint8_t POINTS_PER_FRAME = 12;
38 |     struct __attribute__((__packed__)) RawFrame {
39 |         uint8_t header;                     // 0x54
40 |         uint8_t verLen;                     // 0x2C
41 |         uint16_t speed;                     // Degrees per second
42 |         uint16_t startAngle;                // 0.01 degrees
43 |         RawPoint points[POINTS_PER_FRAME];
44 |         uint16_t endAngle;                  // 0.01 degrees
45 |         uint16_t timestamp;                 // Milliseconds (max 30000)
46 |         uint8_t crc8;
47 |     };
48 | 
49 |     // Stores the remainder of bytes that could not be parsed completely
50 |     static const size_t DATA_REMAINDER_SIZE = 1024;
51 |     uint8_t dataRemainder[DATA_REMAINDER_SIZE];
52 |     uint16_t dataRemainderLen = 0;
53 | 
54 |     uint8_t calCRC8(const uint8_t *p, const size_t len);
55 | 
56 |     // This callback gets called each time we parse out a full scan worth of points
57 |     void (*fullScanCallback)(std::vector<LidarPoint>);
58 | 
59 |     // Temporary storage for points until we get a full scan
60 |     std::vector<LidarPoint> pointBuffer;
61 | 
62 |     // UART related members
63 |     static const size_t UART_BUFFER_SIZE = 256;
64 |     bool readingUart = false;
65 |     int uartFileStream = -1;
66 |     std::thread readingThread;
67 | 
68 |     // Getting more accurate timestamps for points
69 |     uint64_t timestampReferenceWorld = 0;
70 |     uint64_t timestampReferenceLidar = 0;
71 |     uint64_t timestampLidarFramePrevious = 0;
72 | 
73 |     // Reads the UART continuously
74 |     void readLoop();
75 | 
76 | public:
77 |     LD19(void (*fullScanCallback)(std::vector<LidarPoint>)) : fullScanCallback(fullScanCallback) {}
78 |     ~LD19() { stopReading(); }
79 | 
80 |     void parse(uint8_t *data, const size_t len);
81 | 
82 |     bool startReading();
83 |     bool stopReading();
84 | };
85 | 
86 | #endif


--------------------------------------------------------------------------------
/source/example.cpp:
--------------------------------------------------------------------------------
 1 | #include "maus_board.h"
 2 | #include "fhl_ld19.h"
 3 | #include "controller.h"
 4 | 
 5 | void imuDataCallback(const MausBoard::ImuData& imuData) {
 6 |     // Every 10 milliseconds
 7 |     printf("IMU DATA, Yaw: %f radians\n", imuData.getYawRadians());
 8 | 
 9 |     // NOTE! Do not block here. Run longer tasks in a seperate thread. 
10 | }
11 | 
12 | void escTelemetryCallback(const MausBoard::EscTelemetry& escTelemetry) {
13 |     // Every 30 milliseconds
14 |     printf("ESC DATA, Voltage: %f ERPM: %d\n", escTelemetry.getVoltage(), escTelemetry.getERPM());
15 | 
16 |     // NOTE!
17 |     // Due to the nature of the KISS ESC telemetry protocol, ERPM is unsigned. 
18 |     // This means that in order to determine which direction the motor is turning, you will need to compare this to your throttle input.
19 |     // In addition to this, due to the limitations of the ESC, ERPM will be 0 unless throttle is applied. 
20 |     // So ERPM will not be accurate when rolling under 0 throttle.
21 |     // Example: 
22 |     // float throttleValue = -0.8f;
23 |     // const float signedERPM = copysignf(escTelemetry.getERPM(), throttleValue);
24 | 
25 |     // NOTE! Do not block here. Run longer tasks in a seperate thread.
26 | }
27 | 
28 | void ld19FullScanCallback(std::vector<LD19::LidarPoint> points) {
29 |     // Every 100 milliseconds
30 |     printf("FULL SCAN: %d points\n", points.size());
31 | 
32 |     // NOTE! Do not block here. Run longer tasks in a seperate thread.
33 | }
34 | 
35 | int main() {
36 |     // Create an instance and set the callbacks
37 |     MausBoard board(&imuDataCallback, &escTelemetryCallback);
38 |     board.startReading(); // Read data in a separate thread until stopReading() 
39 | 
40 |     // Create an instance and set the callback
41 |     LD19 ld19(&ld19FullScanCallback);
42 |     ld19.startReading(); // Read data in a separate thread until stopReading()
43 | 
44 |     // Set the LED colors (blue, red)
45 |     board.sentSetRGB( {0x000020, 0x200000} );
46 | 
47 |     // Send a "set servos" command to the neutral positions
48 |     board.sendSetServos(1500, 1500);
49 |     // Board will failsafe after 1 second if the SetServos command is not continuously recieved
50 |     // This just means that the default servo positions will be set after 1 second (neutral)
51 |     // In actual use, you should be sending this command at a set interval (I've used 10ms)
52 | 
53 |     // Create a controller object so we can control the car with a joystick
54 |     Controller controller;
55 |     controller.startPolling();
56 | 
57 |     // Set some scalers for throttle and steering
58 |     const float throttleScaler = 0.5f; // Max throttle value from -0.5 to 0.5
59 |     const float steeringScaler = -0.9f; // Max steering value from -0.9 to 0.9 (negative because its reversed on my vehicle)
60 | 
61 |     // Keep reading forever
62 |     while (true) {
63 |         // Grab the current joystick inputs and send them to the servos after scaling
64 |         const float currentThrottleOutput = controller.throttlePos.load() * throttleScaler;
65 |         const float currentSteeringOutput = controller.steeringPos.load() * steeringScaler;
66 |         board.sendSetServos((currentSteeringOutput * 500.0f) + 1500, (currentThrottleOutput * 500.0f) + 1500);
67 | 
68 |         // Sleep for 10 milliseconds
69 |         usleep(10000);
70 |     }
71 | 
72 |     // Ideally if upon exit you should call board.stopReading() and ld19.stopReading()
73 |     // But I haven't been doing that and I haven't seen any issues
74 | }


--------------------------------------------------------------------------------
/source/joystick.h:
--------------------------------------------------------------------------------
  1 | // Licensed under the Apache License, Version 2.0 (the "License");
  2 | // you may not use this file except in compliance with the License.
  3 | // You may obtain a copy of the License at
  4 | //
  5 | //   http://www.apache.org/licenses/LICENSE-2.0
  6 | //
  7 | // Unless required by applicable law or agreed to in writing, software
  8 | // distributed under the License is distributed on an "AS IS" BASIS,
  9 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 10 | // See the License for the specific language governing permissions and
 11 | // limitations under the License.
 12 | //
 13 | // Copyright Drew Noakes 2013-2016
 14 | 
 15 | #ifndef __JOYSTICK_HH__
 16 | #define __JOYSTICK_HH__
 17 | 
 18 | #include <string>
 19 | #include <iostream>
 20 | 
 21 | #define JS_EVENT_BUTTON 0x01 // button pressed/released
 22 | #define JS_EVENT_AXIS   0x02 // joystick moved
 23 | #define JS_EVENT_INIT   0x80 // initial state of device
 24 | 
 25 | /**
 26 |  * Encapsulates all data relevant to a sampled joystick event.
 27 |  */
 28 | class JoystickEvent
 29 | {
 30 | public:
 31 |   /** Minimum value of axes range */
 32 |   static const short MIN_AXES_VALUE = -32768;
 33 | 
 34 |   /** Maximum value of axes range */
 35 |   static const short MAX_AXES_VALUE = 32767;
 36 |   
 37 |   /**
 38 |    * The timestamp of the event, in milliseconds.
 39 |    */
 40 |   unsigned int time;
 41 |   
 42 |   /**
 43 |    * The value associated with this joystick event.
 44 |    * For buttons this will be either 1 (down) or 0 (up).
 45 |    * For axes, this will range between MIN_AXES_VALUE and MAX_AXES_VALUE.
 46 |    */
 47 |   short value;
 48 |   
 49 |   /**
 50 |    * The event type.
 51 |    */
 52 |   unsigned char type;
 53 |   
 54 |   /**
 55 |    * The axis/button number.
 56 |    */
 57 |   unsigned char number;
 58 | 
 59 |   /**
 60 |    * Returns true if this event is the result of a button press.
 61 |    */
 62 |   bool isButton()
 63 |   {
 64 |     return (type & JS_EVENT_BUTTON) != 0;
 65 |   }
 66 | 
 67 |   /**
 68 |    * Returns true if this event is the result of an axis movement.
 69 |    */
 70 |   bool isAxis()
 71 |   {
 72 |     return (type & JS_EVENT_AXIS) != 0;
 73 |   }
 74 | 
 75 |   /**
 76 |    * Returns true if this event is part of the initial state obtained when
 77 |    * the joystick is first connected to.
 78 |    */
 79 |   bool isInitialState()
 80 |   {
 81 |     return (type & JS_EVENT_INIT) != 0;
 82 |   }
 83 | 
 84 |   /**
 85 |    * The ostream inserter needs to be a friend so it can access the
 86 |    * internal data structures.
 87 |    */
 88 |   friend std::ostream& operator<<(std::ostream& os, const JoystickEvent& e);
 89 | };
 90 | 
 91 | /**
 92 |  * Stream insertion function so you can do this:
 93 |  *    cout << event << endl;
 94 |  */
 95 | std::ostream& operator<<(std::ostream& os, const JoystickEvent& e);
 96 | 
 97 | /**
 98 |  * Represents a joystick device. Allows data to be sampled from it.
 99 |  */
100 | class Joystick
101 | {
102 | private:
103 |   
104 |   int _fd;
105 |   
106 | public:
107 |   ~Joystick();
108 | 
109 |   /**
110 |    * Initialises an instance for the first joystick: /dev/input/js0
111 |    */
112 |   Joystick();
113 | 
114 |   /**
115 |    * Initialises an instance for the joystick with the specified,
116 |    * zero-indexed number.
117 |    */
118 |   Joystick(int joystickNumber);
119 | 
120 |   /**
121 |    * Initialises an instance for the joystick device specified.
122 |    */
123 |   Joystick(std::string devicePath);
124 | 
125 |   /**
126 |    * Joystick objects cannot be copied
127 |    */
128 |   Joystick(Joystick const&) = delete;
129 | 
130 |   /**
131 |    * Joystick objects can be moved
132 |    */
133 |   Joystick(Joystick &&) = default;
134 | 
135 |   /**
136 |    * Initialises an instance for the joystick device specified and provide
137 |    * the option of blocking I/O.
138 |    */
139 |   Joystick(std::string devicePath, bool blocking);
140 |  
141 |   /**
142 |    * Returns true if the joystick was found and may be used, otherwise false.
143 |    */
144 |   bool isFound();
145 |   
146 |   /**
147 |    * Attempts to populate the provided JoystickEvent instance with data
148 |    * from the joystick. Returns true if data is available, otherwise false.
149 |    */
150 |   bool sample(JoystickEvent* event);
151 |   
152 |   void openPath(std::string devicePath, bool blocking=true);
153 | };
154 | 
155 | #endif
156 | 


--------------------------------------------------------------------------------
/source/maus_board.h:
--------------------------------------------------------------------------------
  1 | #ifndef __MAUS_BOARD_H__
  2 | #define __MAUS_BOARD_H__
  3 | 
  4 | // Basic driver for my custom Maus board
  5 | // Written by Justin Barrada
  6 | 
  7 | #include <stdint.h>
  8 | #include <stdio.h>
  9 | #include <vector>
 10 | #include <unistd.h>
 11 | #include <fcntl.h>
 12 | #include <fstream>
 13 | #include <termios.h>
 14 | #include <thread>
 15 | #include <string.h>
 16 | #include <cmath>
 17 | 
 18 | #include "timestamp.h"
 19 | 
 20 | #define DEFAULT_SERIAL_MAUS_BOARD "/dev/ttyAMA2"
 21 | 
 22 | class MausBoard {
 23 | public:
 24 |     struct __attribute__((__packed__)) ImuData {
 25 |         uint64_t timestamp; // Nanoseconds since epoch
 26 | 
 27 |         // Quaternion parameters
 28 |         float qX;
 29 |         float qY;
 30 |         float qZ;
 31 |         float qW;
 32 |         
 33 |         // Gyro 
 34 |         // TODO get to RADIANS/SEC units
 35 |         float gyroX;
 36 |         float gyroY;
 37 |         float gyroZ;
 38 | 
 39 |         // Accel
 40 |         // 8192 per G
 41 |         float accelX;
 42 |         float accelY;
 43 |         float accelZ;
 44 | 
 45 |         // Returns the yaw in radians from the quaternion
 46 |         float getYawRadians() const;
 47 | 
 48 |         // Builds an ImuData object from a FIFO packet
 49 |         static ImuData fromFifoPacket(const uint8_t* fifoPacket, const uint8_t fifoPacketSize);
 50 | 
 51 |         // Writes the ImuData object to a stream
 52 |         void writeBytes(std::ofstream& of) const;
 53 | 
 54 |         // Builds an ImuData object from raw bytes
 55 |         static ImuData fromBytes(const char* bytes);
 56 | 
 57 |         static size_t sizeBytes() { return 8 + 16 + 12 + 12; }
 58 |     }; // 48 bytes
 59 | 
 60 |     struct __attribute__((__packed__)) EscTelemetry {
 61 |         uint64_t timestamp; // Nanoseconds since epoch
 62 |         uint8_t temperature; // Celsius
 63 |         uint16_t voltage; // Volts * 100 (100 = 1V)
 64 |         uint16_t current; // Amps * 100 (100 = 1A)
 65 |         uint16_t consumption; // mAh
 66 |         uint16_t ERPM; // Electrical RPM / 100 (100 = 10000 ERPM)
 67 | 
 68 |         static EscTelemetry fromRawData(const uint8_t* rawData, const uint8_t rawDataSize);
 69 | 
 70 |         float getTemperature() const { return temperature; }
 71 |         float getVoltage() const { return voltage / 100.0f; }
 72 | 		float getCurrent() const { return current / 100.0f; }
 73 |         float getConsumption() const { return consumption; }
 74 | 		float getERPM() const { return ERPM; }
 75 |     }; // 18 bytes
 76 | 
 77 | private:
 78 |     static const uint8_t crcTable[256];
 79 | 
 80 |     uint8_t calCRC8(const uint8_t *p, const size_t len);
 81 | 
 82 |     const uint8_t magicBytesMessage[2] = {0x12, 0x34};
 83 | 
 84 |     enum CommandIds : uint8_t {
 85 |         CMD_ECHO_REQUEST = 0xFF,
 86 |         CMD_ECHO_RESPONSE = 0xFE,
 87 |         CMD_SET_RGB = 0x01,
 88 |         CMD_SET_SERVOS = 0x02,
 89 |         CMD_IMU_DUMP = 0x03,
 90 |         CMD_ENCODER_DUMP = 0x04, // UNIMPLEMENTED
 91 |         CMD_ESC_TELEMETRY_DUMP = 0x05
 92 |     };
 93 | 
 94 |     // Message buffer
 95 |     static const size_t MAX_MESSAGE_SIZE = 5 + 255;
 96 |     uint8_t messageBuffer[MAX_MESSAGE_SIZE];
 97 |     uint16_t messageBufferPos = 0;
 98 | 
 99 |     // Callbacks
100 |     void (*imuDataCallback)(const ImuData&);
101 |     void (*escTelemetryCallback)(const EscTelemetry&);
102 | 
103 |     // UART related members
104 |     static const size_t UART_BUFFER_SIZE = 256;
105 |     bool readingUart = false;
106 |     int uartFileStream = -1;
107 |     std::thread readingThread;
108 | 
109 |     // Parses a successfully received payload
110 |     void parsePayload(const uint8_t* payload, const uint8_t payloadSize);
111 | 
112 |     // Try to parse a message from the message buffer
113 |     void parseMessageBuffer();
114 | 
115 |     // Reads the UART continuously
116 |     void readLoop();
117 | 
118 |     // Send a message internally
119 |     void sendMessage(const uint8_t* payload, const uint8_t payloadSize);
120 | 
121 | public:
122 |     // Public debug callback (DEPRECATED)
123 |     void (*echoResponseCallback)(const uint8_t* payload, const uint8_t payloadSize) = nullptr;
124 | 
125 |     MausBoard(void (*imuDataCallback)(const ImuData&), void (*escTelemetryCallback)(const EscTelemetry&)) : imuDataCallback(imuDataCallback), escTelemetryCallback(escTelemetryCallback) {}
126 |     ~MausBoard() { stopReading(); }
127 | 
128 |     bool startReading();
129 |     bool stopReading();
130 | 
131 |     // Send servo and throttle values in servo pulse microseconds (1000 = -100%, 1500 = 0%, 2000 = 100%)
132 |     void sendSetServos(const uint16_t steering, const uint16_t throttle);
133 | 
134 |     // Send a std::vector of uint32_t colors (max of 16) the PCB has 2 LEDs onboard
135 |     void sentSetRGB(const std::vector<uint32_t>& colors);
136 | 
137 |     // Comms debug (DEPRECATED)
138 |     void sendEcho(const uint8_t* data, const uint8_t dataSize);
139 | };
140 | 
141 | #endif


--------------------------------------------------------------------------------
/source/controller.cpp:
--------------------------------------------------------------------------------
  1 | #include "controller.h"
  2 | 
  3 | float Controller::map(const float in, const float inMin, const float inMax, const float outMin, const float outMax) {
  4 |     return (in - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
  5 | }
  6 | 
  7 | bool Controller::isJoystickAvailable() {
  8 |     struct stat buffer;
  9 |     return (stat(DEFAULT_JOYSTICK.c_str(), &buffer) == 0); 
 10 | }
 11 | 
 12 | bool Controller::tryConnect() {
 13 |     // Reset to a safe state
 14 |     disconnected();
 15 | 
 16 |     printf("Attempting to connect controller...\n");
 17 | 
 18 |     // Re-open the joystick
 19 |     joystick.openPath(DEFAULT_JOYSTICK, true);
 20 | 
 21 |     return joystick.isFound();
 22 | }
 23 | 
 24 | void Controller::disconnected() {
 25 |     rightTriggerPos = 0.0f;
 26 |     leftTriggerPos = 0.0f;
 27 |     throttlePos = 0.0f;
 28 |     steeringPos = 0.0f;
 29 | 
 30 |     autonomousModeActive = false;
 31 |     recordingModeActive = false;
 32 |     isConnected = false;
 33 | 
 34 |     aButtonPressed = false;
 35 |     bButtonPressed = false;
 36 |     xButtonPressed = false;
 37 |     yButtonPressed = false;
 38 | 
 39 |     dPadUpPressed = false;
 40 |     dPadDownPressed = false;    
 41 |     dPadLeftPressed = false;
 42 |     dPadRightPressed = false;
 43 | }
 44 | 
 45 | void Controller::poll() {
 46 |     // Check if joystick is connected    
 47 |     uint64_t timestamp = TimeStamp::get();
 48 |     if ((timestamp - lastConnectedCheckTimestamp) > MIN_CONNECTED_CHECK_NSECS) {
 49 |         lastConnectedCheckTimestamp = timestamp;
 50 |         if (!isJoystickAvailable() || !joystick.isFound()) {
 51 |             // Try to connect the joystick if it isn't connected/found
 52 |             isConnected = tryConnect();
 53 |         }
 54 |     }
 55 | 
 56 |     if (joystick.isFound()) {
 57 |         if (!isConnected)
 58 |             isConnected = true;
 59 | 
 60 |         JoystickEvent event;
 61 |         if (joystick.sample(&event)) {
 62 |             if (event.isButton()) {
 63 |                 bool aButtonPressedPrevious = aButtonPressed;
 64 |                 bool bButtonPressedPrevious = bButtonPressed;
 65 |                 bool xButtonPressedPrevious = xButtonPressed;
 66 |                 bool yButtonPressedPrevious = yButtonPressed;
 67 | 
 68 |                 if (event.number == 0)
 69 |                     aButtonPressed = (event.value > 0);
 70 |                 if (event.number == 1)
 71 |                     bButtonPressed = (event.value > 0);
 72 |                 if (event.number == 3)
 73 |                     xButtonPressed = (event.value > 0);
 74 |                 if (event.number == 4)
 75 |                     yButtonPressed = (event.value > 0);
 76 | 
 77 |                 // Toggle autonomous mode
 78 |                 if (!aButtonPressedPrevious && aButtonPressed)
 79 |                     autonomousModeActive = !autonomousModeActive;
 80 | 
 81 |                 // Toggle recording mode
 82 |                 if (!yButtonPressedPrevious && yButtonPressed)
 83 |                     recordingModeActive = !recordingModeActive;
 84 |             }
 85 |             if (event.isAxis()) {
 86 |                 // Left stick x axis
 87 |                 if (event.number == 0) {
 88 |                     steeringPos = map(event.value, JoystickEvent::MIN_AXES_VALUE, JoystickEvent::MAX_AXES_VALUE, -1.0f, 1.0f);
 89 |                 }
 90 |                 
 91 |                 // Right trigger
 92 |                 if (event.number == 4) {
 93 |                     rightTriggerPos = map(event.value, JoystickEvent::MIN_AXES_VALUE, JoystickEvent::MAX_AXES_VALUE, 0.0f, 1.0f);
 94 |                 }
 95 | 
 96 |                 // Left trigger
 97 |                 if (event.number == 5) {
 98 |                     leftTriggerPos = map(event.value, JoystickEvent::MIN_AXES_VALUE, JoystickEvent::MAX_AXES_VALUE, 0.0f, 1.0f);
 99 |                 }
100 | 
101 |                 // DPad L/R
102 |                 if (event.number == 6) {
103 |                     dPadLeftPressed = (event.value < 0);
104 |                     dPadRightPressed = (event.value > 0);
105 |                 }
106 | 
107 |                 // DPad U/D
108 |                 if (event.number == 7) {
109 |                     dPadUpPressed = (event.value < 0);
110 |                     dPadDownPressed = (event.value > 0);
111 |                 }
112 | 
113 |                 // Combine left and right trigger
114 |                 throttlePos = (rightTriggerPos - leftTriggerPos);
115 | 
116 |                 // Cancel autonomous mode if the left trigger is pressed a certain amount
117 |                 if (leftTriggerPos >= autonomousModeCancelLeftTriggerMinimum) {
118 |                     autonomousModeActive = false;
119 |                 }
120 |             }
121 |         }
122 |     } else {
123 |         disconnected();
124 |     }
125 | }
126 | 
127 | void Controller::pollLoop() {
128 |     while (isPolling) {
129 |         poll();
130 |         usleep(POLLING_LOOP_DELAY_USECS);
131 |     }
132 | }
133 | 
134 | bool Controller::startPolling() {
135 |     if (!isPolling) {
136 |         isPolling = true;
137 |         pollingThread = std::thread(&Controller::pollLoop, this);
138 |         return true;
139 |     } else {
140 |         printf("Cannot start polling. Controller is already polling\n");
141 |         return false;
142 |     }
143 | }
144 | 
145 | bool Controller::stopPolling() {
146 |     if (isPolling) {
147 |         isPolling = false;
148 |         pollingThread.join();
149 |         disconnected();
150 | 
151 |         return true;
152 |     }
153 |     return false;
154 | }


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
  1 | > [!NOTE]
  2 | > This repository is just for the PCB, firmware and related driver code. The code relating to the control, navigation, SLAM, etc of my specific implementation for the car will remain private, for now. This project is intended to make it easier and cheaper for people to get started developing their own autonomous vehicle code.  
  3 | 
  4 | ## MAUS Board
  5 | This project was developed as a means to create a very inexpensive F1Tenth car. It is a Raspberry Pi “hat” that connects various peripherals required for a functioning autonomous vehicle.
  6 | 
  7 | In its current state it has connections for:
  8 | - Serial LiDAR device (FHL-LD19)
  9 | - I2C IMU (MPU6050)
 10 | - Neopixel RGB LEDs (two LEDs onboard)
 11 | - Standard servo and ESC connectors
 12 | 
 13 | In addition to that, various 5V and 3.3V power connections, spare I2C pads, an external RGB strip connection, and test points for any unused ESP32 pins.
 14 | 
 15 | > [!WARNING]
 16 | > In order to make this board useful to the majority of people using ROS, I would need to provide a ROS driver for this board. I know next to nothing about ROS and I would appreciate any help writing a ROS driver for this. See source/example.cpp for how to communicate with the board.
 17 | 
 18 | ![image](https://github.com/user-attachments/assets/33a185bd-26c8-4e59-aa50-dd889c8ff06f)
 19 | 
 20 | ![image](https://github.com/user-attachments/assets/db537b5e-f5b3-458c-80c6-88dc17858f35)
 21 | 
 22 | ![image](https://github.com/user-attachments/assets/b86efd99-d0ee-48d6-b2f3-cb1050980544)
 23 | 
 24 | Video:
 25 | https://www.youtube.com/watch?v=Ug7xEspHlwM
 26 | 
 27 | 
 28 | ### Disclaimer
 29 | I am only doing this as a hobby, and this project is very much DIY and not a complete solution. Please use the code and hardware with caution.
 30 | 
 31 | ---
 32 | 
 33 | ### TODOs
 34 | - ROS driver
 35 | - Bidirectional DShot instead of seperate ESC telemetry wire
 36 | 
 37 | ---
 38 | 
 39 | ### Supported ESCS
 40 | Any ESC that supports an external telemetry wire will work (BLHeli32 or AM32 firmware). It also must have a 5V BEC that can provide at least 3A to power the Pi and board.
 41 | 
 42 | Recommended ESC: https://www.amazon.com/dp/B0CPXXPPFH
 43 | 
 44 | ### Configuring the ESC
 45 | AM32 ESC firmware is initally intended to be used for drones, so it will require a little bit of configuration before it can be used with this project. The easiest way I found to configure the ESC is to connect it to a Betaflight flight controller since it provides a passthrough interface for configuring the ESC.
 46 | 
 47 | I saw [this little board on Amazon priced around $11](https://www.amazon.com/dp/B0CCXGFSB3/) that claims it can configure AM32 ESCs but I don't know how it works or how to use it. If you end up getting on please contact me so I can help out and update this documentation. 
 48 | 
 49 | TODO CONFIGURATION
 50 | 
 51 | ---
 52 | 
 53 | ### Raspberry Pi Setup (Raspberry Pi 4):
 54 | Note: For Raspberry Pi 5 setup, see the section below
 55 | 
 56 | #### Assuming a fresh install of Raspbian lite:
 57 | `sudo apt update`
 58 | 
 59 | `sudo apt upgrade`
 60 | 
 61 | #### Enable the additional UARTs:
 62 | `sudo raspi-config`
 63 | 
 64 | Interface Options -> Serial Port -> Shell? No -> Enable Hardware? Yes
 65 | 
 66 | Exit -> Reboot? No
 67 | 
 68 | #### Modify config.txt
 69 | `sudo nano /boot/firmware/config.txt`
 70 | 
 71 | Add the following lines at the end, before the `enable_uart=1` line
 72 | 
 73 | `dtoverlay=uart2`
 74 | 
 75 | `dtoverlay=uart3`
 76 | 
 77 | Save and exit nano (ctrl+s, ctrl+x)
 78 | 
 79 | `sudo reboot`
 80 | 
 81 | ---
 82 | 
 83 | ### Raspberry Pi Setup (Raspberry Pi 5):
 84 | #### Assuming a fresh install of Raspbian lite:
 85 | `sudo apt update`
 86 | 
 87 | `sudo apt upgrade`
 88 | 
 89 | `sudo rpi-update`
 90 | 
 91 | #### Enable the additional UARTs:
 92 | `sudo raspi-config`
 93 | 
 94 | Interface Options -> Serial Port -> Shell? No -> Enable Hardware? Yes
 95 | 
 96 | Exit -> Reboot? No
 97 | 
 98 | #### Modify config.txt
 99 | `sudo nano /boot/firmware/config.txt`
100 | 
101 | Add the following lines at the end: 
102 | 
103 | `enable_uart=1`
104 | 
105 | `dtoverlay=uart0`
106 | 
107 | `dtoverlay=uart1`
108 | 
109 | Save and exit nano (ctrl+s, ctrl+x)
110 | 
111 | `sudo reboot`
112 | 
113 | Edit the source files with pi5 specific serial interfaces:
114 | 
115 | in `source/fhl_ld19.h` change `/dev/serial0` to `/dev/ttyAMA0`
116 | 
117 | in `source/maus_board.h` change `/dev/ttyAMA2` to `/dev/ttyAMA1`
118 | 
119 | ---
120 | 
121 | ### Running the example
122 | Clone this repo to the Pi
123 | 
124 | CD into the source folder
125 | 
126 | `make example`
127 | 
128 | `./example`
129 | 
130 | --- 
131 | 
132 | ### BOM
133 | Here's a Bill Of Materials for the parts I used in my car:
134 | | Part            | Link                                         | Price   |
135 | |-----------------|----------------------------------------------|---------|
136 | | Chassis         | https://www.amazon.com/gp/product/B0CN9Q8LMN | $170.00 |
137 | | Raspberry Pi 4B | https://www.amazon.com/dp/B07TC2BK1X         |  $62.00 |
138 | | LiDAR           | https://www.amazon.com/dp/B0B1V8D36H         |  $70.00 |
139 | | IMU             | https://www.amazon.com/dp/B08FHXQ58X         |  $12.00 |
140 | | Brushless Motor | https://www.amazon.com/gp/product/B0B1D3K8TP |  $23.00 |
141 | | ESC             | https://www.amazon.com/gp/product/B0CPXXPPFH |  $50.00 |
142 | | ESC Plugs       | https://www.amazon.com/gp/product/B08LL8HPHR |   $8.00 |
143 | | MAUS Board      |                                              | ~$30?   |
144 | | TOTAL           |                                              | ~$425   |
145 | 
146 | You can save a lot money if you already have some similar parts. I still don't know what to price that MAUS board at, so $30 is a placeholder.
147 | 
148 | I chose that chassis because it was the cheapest I could find that was ready to run in 1/10 scale form factor. I also have CAD files for a custom mounting plate for all the electronics that you can laser cut/cnc. (TODO include that file). If you find a cheaper chassis or one that already has a brushless motor, please let me know so I can update the BOM. 
149 | 
150 | If your chassis already has a brushless motor, you don't need to buy the motor. And if you don't care about getting ESC telemetry (ERPM, battery voltage, etc) then you also don't need to buy any of the ESC related things and you can use the one that came with your chassis. Although having ERPM information can make localization and dead-reckoning better in your implementation.
151 | 
152 | If you do decide to get the ESC, it does not come with a battery connector, so you will have to solder on one that matches your battery. In addition to that you will need to solder on a wire to the telemetry pad on the ESC to connect to the "ESC TELEM" pin on the board.
153 | 
154 | The ESC runs on AM32 firmware which is actively maintained and provides some cool features that make it easier to use for this project. We will need to configure the ESC to send telemetry and respond to servo commands like a typical RC car ESC. ESCs running BLHeli32 firmware will also work as they have the same features and configuration but BLHeli is no longer maintained so I don't recommened it.
155 | 
156 | See [Confguring the ESC](#Configuring-the-ESC) for more info. You will probably need to buy something else in order to configure the ESC ~$10. 
157 | 
158 | ---
159 | 
160 | ### Contact
161 | https://www.instagram.com/jus10barrada/
162 | 
163 | https://www.linkedin.com/in/justin-barrada-861546a1/
164 | 
165 | 
166 | 


--------------------------------------------------------------------------------
/ESP32_firmware/main/messaging.h:
--------------------------------------------------------------------------------
  1 | #ifndef __MESSAGING_H__
  2 | #define __MESSAGING_H__
  3 | 
  4 | // Custom messaging protocol between the ESP32 and RPi. Allows for dynamic payload sizes and has a checksum for data security.
  5 | 
  6 | #include <stddef.h>
  7 | #include <stdint.h>
  8 | 
  9 | #define MAX_MESSAGE_SIZE (5 + 255)
 10 | 
 11 | // -- MESSAGING --
 12 | // Message format
 13 | // 0x12 - magic 
 14 | // 0x34 - magic
 15 | // 0x?? - message ID (wraps)
 16 | // 0x?? - payload size
 17 | // 0x?? - CRC8 of payload
 18 | // 0x?? - payload
 19 | // 0x?? - payload ...
 20 | 
 21 | // Acknowledge format (UNIMPLEMENTED. NOT NEEDED)
 22 | // 0x56 - magic
 23 | // 0x78 - magic
 24 | // 0x?? - message id of acknowledged message
 25 | // 0x?? - CRC8 of magic and message id
 26 | 
 27 | // CRC8 poly: 0x31
 28 | static const uint8_t crc8_table[] = {
 29 |   0x00, 0x31, 0x62, 0x53, 0xc4, 0xf5, 0xa6, 0x97, 0xb9, 0x88, 0xdb, 0xea, 0x7d,
 30 |   0x4c, 0x1f, 0x2e, 0x43, 0x72, 0x21, 0x10, 0x87, 0xb6, 0xe5, 0xd4, 0xfa, 0xcb,
 31 |   0x98, 0xa9, 0x3e, 0x0f, 0x5c, 0x6d, 0x86, 0xb7, 0xe4, 0xd5, 0x42, 0x73, 0x20,
 32 |   0x11, 0x3f, 0x0e, 0x5d, 0x6c, 0xfb, 0xca, 0x99, 0xa8, 0xc5, 0xf4, 0xa7, 0x96,
 33 |   0x01, 0x30, 0x63, 0x52, 0x7c, 0x4d, 0x1e, 0x2f, 0xb8, 0x89, 0xda, 0xeb, 0x3d,
 34 |   0x0c, 0x5f, 0x6e, 0xf9, 0xc8, 0x9b, 0xaa, 0x84, 0xb5, 0xe6, 0xd7, 0x40, 0x71,
 35 |   0x22, 0x13, 0x7e, 0x4f, 0x1c, 0x2d, 0xba, 0x8b, 0xd8, 0xe9, 0xc7, 0xf6, 0xa5,
 36 |   0x94, 0x03, 0x32, 0x61, 0x50, 0xbb, 0x8a, 0xd9, 0xe8, 0x7f, 0x4e, 0x1d, 0x2c,
 37 |   0x02, 0x33, 0x60, 0x51, 0xc6, 0xf7, 0xa4, 0x95, 0xf8, 0xc9, 0x9a, 0xab, 0x3c,
 38 |   0x0d, 0x5e, 0x6f, 0x41, 0x70, 0x23, 0x12, 0x85, 0xb4, 0xe7, 0xd6, 0x7a, 0x4b,
 39 |   0x18, 0x29, 0xbe, 0x8f, 0xdc, 0xed, 0xc3, 0xf2, 0xa1, 0x90, 0x07, 0x36, 0x65,
 40 |   0x54, 0x39, 0x08, 0x5b, 0x6a, 0xfd, 0xcc, 0x9f, 0xae, 0x80, 0xb1, 0xe2, 0xd3,
 41 |   0x44, 0x75, 0x26, 0x17, 0xfc, 0xcd, 0x9e, 0xaf, 0x38, 0x09, 0x5a, 0x6b, 0x45,
 42 |   0x74, 0x27, 0x16, 0x81, 0xb0, 0xe3, 0xd2, 0xbf, 0x8e, 0xdd, 0xec, 0x7b, 0x4a,
 43 |   0x19, 0x28, 0x06, 0x37, 0x64, 0x55, 0xc2, 0xf3, 0xa0, 0x91, 0x47, 0x76, 0x25,
 44 |   0x14, 0x83, 0xb2, 0xe1, 0xd0, 0xfe, 0xcf, 0x9c, 0xad, 0x3a, 0x0b, 0x58, 0x69,
 45 |   0x04, 0x35, 0x66, 0x57, 0xc0, 0xf1, 0xa2, 0x93, 0xbd, 0x8c, 0xdf, 0xee, 0x79,
 46 |   0x48, 0x1b, 0x2a, 0xc1, 0xf0, 0xa3, 0x92, 0x05, 0x34, 0x67, 0x56, 0x78, 0x49,
 47 |   0x1a, 0x2b, 0xbc, 0x8d, 0xde, 0xef, 0x82, 0xb3, 0xe0, 0xd1, 0x46, 0x77, 0x24,
 48 |   0x15, 0x3b, 0x0a, 0x59, 0x68, 0xff, 0xce, 0x9d, 0xac};
 49 | 
 50 | uint8_t crc8(uint8_t crc, const uint8_t *mem, size_t len) {
 51 |     const uint8_t *data = mem;
 52 |     if (data == NULL)
 53 |         return 0xff;
 54 |     crc &= 0xff;
 55 |     for (size_t i = 0; i < len; i++) 
 56 |         crc = crc8_table[crc ^ data[i]];
 57 |     return crc;
 58 | }
 59 | 
 60 | class MessageInterface {
 61 | private:
 62 |     const uint8_t magicBytesMessage[2] = {0x12, 0x34};
 63 |     const uint8_t magicBytesAck[2] = {0x56, 0x78};
 64 | 
 65 |     // Circular message buffer, after messages are parsed they are "blanked" out with zeros
 66 |     uint8_t messageBuffer[MAX_MESSAGE_SIZE];
 67 |     uint16_t messageBufferPos = 0;
 68 | 
 69 |     uint8_t currentMessageId = 0;
 70 | 
 71 |     // Stream to use
 72 |     Stream &serialPort;
 73 | 
 74 |     // Message received callback, passes the payload and payload size
 75 |     bool (*messageReceivedCallback)(const uint8_t*,const uint16_t);
 76 | public:
 77 |     MessageInterface(Stream &serialPort, bool (*messageReceivedCallback)(const uint8_t*, const uint16_t)) : serialPort(serialPort), messageReceivedCallback(messageReceivedCallback) {}
 78 | 
 79 |     void tryParseMessageBuffer() {
 80 |         // Offset relative to messageBufferPos
 81 |         uint16_t offset = 0;
 82 | 
 83 |         // Positions within magic bytes
 84 |         uint8_t magicPosMessage = 0;
 85 |         uint8_t magicPosAck = 0;
 86 | 
 87 |         while (offset < MAX_MESSAGE_SIZE) {
 88 |             // Get the actual possition in messageBufferPos
 89 |             const uint16_t bufferPos = (offset + messageBufferPos) % MAX_MESSAGE_SIZE;
 90 |             offset++;
 91 | 
 92 |             magicPosMessage = (magicBytesMessage[magicPosMessage] == messageBuffer[bufferPos]) ? magicPosMessage + 1 : 0;
 93 |             magicPosAck = (magicBytesAck[magicPosAck] == messageBuffer[bufferPos]) ? magicPosAck + 1 : 0;
 94 | 
 95 |             // Check if we have magic bytes for a regular message
 96 |             if (magicPosMessage == 2) {
 97 |                 magicPosMessage = 0;
 98 |                 // 2 byte magic number
 99 |                 // 1 byte message ID
100 |                 // 1 byte payload size
101 |                 // 1 byte payload CRC8
102 |                 
103 |                 // Check if there is enough room to read up to the CRC
104 |                 if (offset + 3 <= MAX_MESSAGE_SIZE) {
105 |                     const uint8_t messageId = messageBuffer[(bufferPos + 1) % MAX_MESSAGE_SIZE];
106 |                     const uint8_t payloadSize = messageBuffer[(bufferPos + 2) % MAX_MESSAGE_SIZE];
107 |                     const uint8_t payloadCrc8 = messageBuffer[(bufferPos + 3) % MAX_MESSAGE_SIZE];
108 | 
109 |                     // Check if there is enough room to read the whole message
110 |                     if ((offset + 3 + payloadSize) <= MAX_MESSAGE_SIZE) {
111 |                         // Extract the payload
112 |                         uint8_t payload[255];
113 |                         for (uint16_t payloadIndex = 0; payloadIndex < payloadSize; payloadIndex++) {
114 |                             const uint16_t payloadBufferPos = (bufferPos + 4 + payloadIndex) % MAX_MESSAGE_SIZE;
115 |                             payload[payloadIndex] = messageBuffer[payloadBufferPos];
116 |                         }
117 | 
118 |                         // Check the CRC8
119 |                         const uint8_t payloadCrc8Calc = crc8(0, payload, payloadSize);
120 |                         if (payloadCrc8Calc == payloadCrc8) {
121 |                             // We have a message!
122 |                             const bool result = messageReceivedCallback(payload, payloadSize);                            
123 |                             
124 |                             // TODO send an ack
125 | 
126 |                             // Zero out the message so we dont parse it again later
127 |                             for (uint16_t messageIndex = 0; messageIndex < (payloadSize + 5); messageIndex++) {
128 |                                 const uint16_t zeroBufferPos = (bufferPos + MAX_MESSAGE_SIZE - 1 + messageIndex) % MAX_MESSAGE_SIZE;
129 |                                 messageBuffer[zeroBufferPos] = 0x00;
130 |                             }
131 | 
132 |                             // Advance the offset
133 |                             offset += (payloadSize + 4);
134 |                         }
135 |                     }
136 |                 }
137 |             }
138 |         }
139 |     }
140 | 
141 |     void update() {
142 |         if (serialPort.available()) {
143 |             // Keep pushing data into the mesasage buffer
144 |             while (serialPort.available()) {
145 |                 messageBuffer[messageBufferPos] = serialPort.read();
146 |                 messageBufferPos = (messageBufferPos + 1) % MAX_MESSAGE_SIZE;
147 |             }
148 | 
149 |             // Check if we can parse out a message
150 |             tryParseMessageBuffer();
151 |         }
152 |     }
153 | 
154 |     void sendMessage(const uint8_t* payload, const uint8_t payloadSize) {
155 |         // Write the magic bytes
156 |         serialPort.write(magicBytesMessage, 2);
157 | 
158 |         // Write the message ID
159 |         serialPort.write(currentMessageId);
160 |         currentMessageId = (currentMessageId + 1) % 256;
161 | 
162 |         // Write the payload size
163 |         serialPort.write(payloadSize);
164 | 
165 |         // Write the CRC8
166 |         const uint8_t payloadCrc8Calc = crc8(0, payload, payloadSize);
167 |         serialPort.write(payloadCrc8Calc);
168 | 
169 |         // Write the payload
170 |         serialPort.write(payload, payloadSize);
171 |     }
172 | };
173 | 
174 | #endif


--------------------------------------------------------------------------------
/source/fhl_ld19.cpp:
--------------------------------------------------------------------------------
  1 | #include "fhl_ld19.h"
  2 | 
  3 | const uint8_t LD19::crcTable[] = {
  4 |     0x00, 0x4D, 0x9A, 0xD7, 0x79, 0x34, 0xE3, 0xAE, 0xF2, 0xBF, 0x68, 0x25, 0x8B, 0xC6, 0x11, 0x5C,
  5 |     0xA9, 0xE4, 0x33, 0x7E, 0xD0, 0x9D, 0x4A, 0x07, 0x5B, 0x16, 0xC1, 0x8C, 0x22, 0x6F, 0xB8, 0xF5,
  6 |     0x1F, 0x52, 0x85, 0xC8, 0x66, 0x2B, 0xFC, 0xB1, 0xED, 0xA0, 0x77, 0x3A, 0x94, 0xD9, 0x0E, 0x43,
  7 |     0xB6, 0xFB, 0x2C, 0x61, 0xCF, 0x82, 0x55, 0x18, 0x44, 0x09, 0xDE, 0x93, 0x3D, 0x70, 0xA7, 0xEA,
  8 |     0x3E, 0x73, 0xA4, 0xE9, 0x47, 0x0A, 0xDD, 0x90, 0xCC, 0x81, 0x56, 0x1B, 0xB5, 0xF8, 0x2F, 0x62,
  9 |     0x97, 0xDA, 0x0D, 0x40, 0xEE, 0xA3, 0x74, 0x39, 0x65, 0x28, 0xFF, 0xB2, 0x1C, 0x51, 0x86, 0xCB,
 10 |     0x21, 0x6C, 0xBB, 0xF6, 0x58, 0x15, 0xC2, 0x8F, 0xD3, 0x9E, 0x49, 0x04, 0xAA, 0xE7, 0x30, 0x7D,
 11 |     0x88, 0xC5, 0x12, 0x5F, 0xF1, 0xBC, 0x6B, 0x26, 0x7A, 0x37, 0xE0, 0xAD, 0x03, 0x4E, 0x99, 0xD4,
 12 |     0x7C, 0x31, 0xE6, 0xAB, 0x05, 0x48, 0x9F, 0xD2, 0x8E, 0xC3, 0x14, 0x59, 0xF7, 0xBA, 0x6D, 0x20,
 13 |     0xD5, 0x98, 0x4F, 0x02, 0xAC, 0xE1, 0x36, 0x7B, 0x27, 0x6A, 0xBD, 0xF0, 0x5E, 0x13, 0xC4, 0x89,
 14 |     0x63, 0x2E, 0xF9, 0xB4, 0x1A, 0x57, 0x80, 0xCD, 0x91, 0xDC, 0x0B, 0x46, 0xE8, 0xA5, 0x72, 0x3F,
 15 |     0xCA, 0x87, 0x50, 0x1D, 0xB3, 0xFE, 0x29, 0x64, 0x38, 0x75, 0xA2, 0xEF, 0x41, 0x0C, 0xDB, 0x96,
 16 |     0x42, 0x0F, 0xD8, 0x95, 0x3B, 0x76, 0xA1, 0xEC, 0xB0, 0xFD, 0x2A, 0x67, 0xC9, 0x84, 0x53, 0x1E,
 17 |     0xEB, 0xA6, 0x71, 0x3C, 0x92, 0xDF, 0x08, 0x45, 0x19, 0x54, 0x83, 0xCE, 0x60, 0x2D, 0xFA, 0xB7,
 18 |     0x5D, 0x10, 0xC7, 0x8A, 0x24, 0x69, 0xBE, 0xF3, 0xAF, 0xE2, 0x35, 0x78, 0xD6, 0x9B, 0x4C, 0x01,
 19 |     0xF4, 0xB9, 0x6E, 0x23, 0x8D, 0xC0, 0x17, 0x5A, 0x06, 0x4B, 0x9C, 0xD1, 0x7F, 0x32, 0xE5, 0xA8
 20 | };
 21 | 
 22 | uint8_t LD19::calCRC8(const uint8_t *p, const size_t len) {
 23 |     uint8_t crc = 0;
 24 |     for (size_t i = 0; i < len; i++)
 25 |         crc = crcTable[(crc ^ p[i]) & 0xff];
 26 |     return crc;
 27 | }
 28 | 
 29 | void LD19::parse(uint8_t *data, const size_t len) {
 30 |     // Construct the current datablock including the remainder
 31 |     const size_t bufferLen = len + dataRemainderLen;
 32 |     uint8_t buffer[bufferLen];
 33 |     memcpy(buffer, dataRemainder, dataRemainderLen);
 34 |     memcpy(buffer + dataRemainderLen, data, len);
 35 | 
 36 |     // Parse RawFrames out of buffer
 37 |     size_t dataRemainderBegin = 0;
 38 |     if (bufferLen >= sizeof(RawFrame)) {
 39 |         size_t bufferPos = 0;
 40 |         while (bufferPos < (bufferLen - sizeof(RawFrame))) {
 41 |             RawFrame* currentFrame = reinterpret_cast<RawFrame*>(&buffer[bufferPos]);
 42 |             
 43 |             // Frame header will always be 0x54, and at the time of writing, the version will be 0x2C
 44 |             if (currentFrame->header == 0x54 && currentFrame->verLen == 0x2C) {
 45 |                 // Check the CRC
 46 |                 uint8_t calculatedCrc8 = calCRC8(&buffer[bufferPos], sizeof(RawFrame) - 1);
 47 |                 if (calculatedCrc8 == currentFrame->crc8) {
 48 |                     // We have a good frame! Get the timestamp
 49 |                     const uint64_t timestamp = TimeStamp::get();
 50 |                     
 51 |                     // Sometimes we can get frames that wrap back around to 0 degrees, add 36000 to the end angle 
 52 |                     if (currentFrame->endAngle < currentFrame->startAngle) 
 53 |                         currentFrame->endAngle += 36000;                
 54 | 
 55 |                     float angleStep = (float)(currentFrame->endAngle - currentFrame->startAngle) / (float)(POINTS_PER_FRAME - 1);
 56 |                     for (uint8_t pointIndex = 0; pointIndex < POINTS_PER_FRAME; pointIndex++) {
 57 |                         LidarPoint point;
 58 |                         point.distance = currentFrame->points[pointIndex].distance;
 59 |                         point.intensity = currentFrame->points[pointIndex].intensity;
 60 | 
 61 |                         // Caclculate the angle for the point
 62 |                         point.angle = (currentFrame->startAngle + (uint16_t)(angleStep * pointIndex)) % 36000;
 63 | 
 64 |                         // Calculate the timestamp for the point (assume the last point is from the current timestamp)
 65 |                         double timestampOffsetSecs = ((double)(angleStep * ((POINTS_PER_FRAME - 1) - pointIndex)) / 100.0) / (double)(currentFrame->speed);
 66 |                         point.timestamp = timestamp - (uint64_t)(timestampOffsetSecs * 1000000000);
 67 | 
 68 |                         // Collect point
 69 |                         pointBuffer.push_back(point);
 70 |                     }
 71 |                     
 72 |                     // Advance bufferPos
 73 |                     bufferPos += sizeof(RawFrame);
 74 | 
 75 |                     // Check if we couldn't parse any bytes
 76 |                     int16_t unparsedBytes = (bufferPos - dataRemainderBegin) - sizeof(RawFrame);
 77 |                     if (unparsedBytes > 0) {
 78 |                         printf("Unable to parse %d bytes\n", unparsedBytes);
 79 |                     }
 80 |                     dataRemainderBegin = bufferPos;                
 81 | 
 82 |                     continue;
 83 |                 } else {
 84 |                     printf("CRC fail: calculated 0x%02x, actual 0x%02x\n", calculatedCrc8, currentFrame->crc8);
 85 |                 }
 86 |             }
 87 |             // If we couldn't parse a frame, advance until we see the start of a frame
 88 |             bufferPos++;        
 89 |         }
 90 |     }
 91 | 
 92 |     // Update new dataRemainder
 93 |     dataRemainderLen = bufferLen - dataRemainderBegin;
 94 |     if (dataRemainderLen > DATA_REMAINDER_SIZE) {
 95 |         int16_t unparsedBytes = dataRemainderLen - DATA_REMAINDER_SIZE;
 96 |         printf("Unable to parse %d bytes. Remainder too large.\n", unparsedBytes);
 97 |         dataRemainderLen = DATA_REMAINDER_SIZE;
 98 |     }
 99 |     if (dataRemainderLen > 0) {
100 |         memcpy(dataRemainder, buffer + dataRemainderBegin, dataRemainderLen);
101 |     }
102 | 
103 |     // Check pointBuffer for a full scan
104 |     size_t lastScanStartIndex = 0;
105 |     uint16_t lastAngle = 0;
106 |     for (size_t i = 0; i < pointBuffer.size(); i++) {
107 |         // Criteria for a scan (point angle goes from ~360 to 0)
108 |         if (i > 0 && lastAngle > pointBuffer[i].angle) {
109 |             // Call the full scan callback with the points vector
110 |             std::vector<LidarPoint> scanPoints(pointBuffer.begin() + lastScanStartIndex, pointBuffer.begin() + i);
111 |             fullScanCallback(scanPoints);
112 | 
113 |             lastScanStartIndex = i;
114 |         }
115 |         lastAngle = pointBuffer[i].angle;
116 |     }
117 |     if (lastScanStartIndex > 0) {
118 |         pointBuffer.erase(pointBuffer.begin(), pointBuffer.begin() + lastScanStartIndex);
119 |     }
120 | }
121 | 
122 | void LD19::readLoop() {
123 |     // Read forever
124 |     if (uartFileStream != -1) {
125 |         uint8_t uartBuffer[UART_BUFFER_SIZE];
126 |         while (readingUart) {
127 |             // Read and parse data
128 |             int len = read(uartFileStream, uartBuffer, UART_BUFFER_SIZE);
129 |             if (len > 0) {
130 |                 parse(uartBuffer, len);
131 |             }
132 |         }
133 |     }
134 | 
135 |     // Close the UART
136 |     close(uartFileStream);
137 | }
138 | 
139 | bool LD19::startReading() {
140 |     if (!readingUart) {
141 |         // Open the UART
142 |         uartFileStream = open(DEFAULT_SERIAL_FHL_LD19, O_RDONLY);
143 |         if (uartFileStream == -1) {
144 |             printf("Unable to open UART\n");
145 |             return false;
146 |         }
147 | 
148 |         // Configure the UART (Flags defined in /usr/include/termios.h - see http://pubs.opengroup.org/onlinepubs/007908799/xsh/termios.h.html)
149 |         struct termios options;
150 |         tcgetattr(uartFileStream, &options);
151 |         options.c_cflag = B230400 | CS8 | CLOCAL | CREAD; // Set baud rate
152 |         options.c_iflag = IGNPAR;
153 |         options.c_oflag = 0;
154 |         options.c_lflag = 0;
155 |         tcflush(uartFileStream, TCIFLUSH);
156 |         tcsetattr(uartFileStream, TCSANOW, &options);
157 | 
158 |         readingUart = true;
159 |         readingThread = std::thread(&LD19::readLoop, this);
160 | 
161 |         return true;
162 |     } else {
163 |         printf("Cannot start reading. UART is already being read\n");
164 |         return false;
165 |     }
166 | }
167 | 
168 | bool LD19::stopReading() {
169 |     if (readingUart) {
170 |         readingUart = false;
171 |         readingThread.join();
172 |         uartFileStream = -1;
173 | 
174 |         return true;
175 |     }
176 |     return false;
177 | }
178 | 


--------------------------------------------------------------------------------
/ESP32_firmware/main/main.ino:
--------------------------------------------------------------------------------
  1 | #include <ESP32Servo.h>
  2 | #include <Adafruit_NeoPixel.h>
  3 | #include <HardwareSerial.h>
  4 | #include "MPU6050_6Axis_MotionApps20.h"
  5 | 
  6 | #include "messaging.h"
  7 | #include "esc_telemetry.h"
  8 | 
  9 | // -- HARDWARE --
 10 | #define PIN_MPU6050_INT 15
 11 | #define PIN_STEERING_SERVO 18
 12 | #define PIN_THROTTLE_SERVO 19
 13 | #define PIN_RGB 13
 14 | #define PIN_PI_UART_RX 16
 15 | #define PIN_PI_UART_TX 17
 16 | #define PIN_ESC_UART_RX 4
 17 | #define PIN_ESC_UART_TX 2
 18 | 
 19 | #define NUM_LEDS 2
 20 | 
 21 | // -- PAYLOAD --
 22 | // First byte is command ID
 23 | // Remaining bytes are command specific
 24 | 
 25 | // -- PAYLOAD COMMANDS --
 26 | #define CMD_ECHO_REQUEST 0xFF
 27 | #define CMD_ECHO_RESPONSE 0xFE
 28 | 
 29 | #define CMD_SET_RGB 0x01
 30 | // 0x01 - set rgb       - sent by the pi to set the RGB strip
 31 | //                      - payload uint32_t color x 16 leds
 32 | 
 33 | #define CMD_SET_SERVOS 0x02
 34 | // 0x02 - set servos    - sent by the pi to set the current servo positions
 35 | //                      - payload uint16_t steering servo, uint16_t throttle servo (microseconds pulse length)
 36 | 
 37 | #define CMD_IMU_DUMP 0x03
 38 | // 0x03 - imu dump      - sent to the pi with IMU data
 39 | //                      - payload DMP 2.0 default FIFO packet
 40 | 
 41 | #define CMD_ENCODER_DUMP 0x04 // UNIMPLEMENTED IN THIS VERSION
 42 | // 0x04 - encoder dump  - sent to the pi with encoder data
 43 | //                      - payload 2x AS5600 packet
 44 | 
 45 | #define CMD_ESC_TELEMETRY_DUMP 0x05
 46 | // 0x05 - telemetry dump  - sent to the pi with ESC telemetry data
 47 | //                        - payload 10 byte ESC telemetry buffer
 48 | 
 49 | // Debug switch
 50 | const bool debugMode = false;
 51 | 
 52 | // Callback declaration
 53 | bool piMessageReceived(const uint8_t* payload, const uint16_t payloadSize);
 54 | 
 55 | // UARTs
 56 | HardwareSerial escTelemetryUART(1);
 57 | HardwareSerial piUART(2);
 58 | 
 59 | // Message interface
 60 | MessageInterface piMessaging(piUART, piMessageReceived);
 61 | 
 62 | // RGB
 63 | Adafruit_NeoPixel rgb(NUM_LEDS, PIN_RGB, NEO_GRB + NEO_KHZ800);
 64 | 
 65 | // Servos
 66 | const uint16_t servoMicrosMin = 1000;
 67 | const uint16_t servoMicrosMax = 2000;
 68 | const uint16_t steeringServoDefaultMicros = 1500;
 69 | const uint16_t throttleServoDefaultMicros = 1500;
 70 | uint16_t steeringServoMicrosNew = 1500;
 71 | uint16_t throttleServoMicrosNew = 1500;
 72 | Servo steeringServo;
 73 | Servo throttleServo;
 74 | bool newServoData = false;
 75 | uint32_t lastSetServoMillis = 0;
 76 | const uint32_t maxSetServoIntervalMillis = 1000;
 77 | 
 78 | // ESC Telemetry
 79 | const bool ESC_TELEMETRY_ENABLED = true;
 80 | ESCTelemetry escTelemetry(escTelemetryUART);
 81 | 
 82 | // MPU6050
 83 | MPU6050 mpu;
 84 | bool dmpReady = false;
 85 | uint16_t packetSize = 0;
 86 | uint8_t fifoBuffer[64];
 87 | 
 88 | // To be completely honest, I don't know if this is required. I just saw this in the convoluted MPU6050 motionapps example
 89 | volatile bool mpuInterrupt = false;
 90 | void dmpDataReady() {
 91 |     mpuInterrupt = true;
 92 | }
 93 | 
 94 | void initMPU6050() {
 95 |     // Initialize MPU
 96 |     mpu.initialize();
 97 |     pinMode(PIN_MPU6050_INT, INPUT);
 98 | 
 99 |     uint8_t devStatus = mpu.dmpInitialize();
100 | 
101 |     // Supply your own gyro offsets here, scaled for min sensitivity
102 |     mpu.setXGyroOffset(220);
103 |     mpu.setYGyroOffset(76);
104 |     mpu.setZGyroOffset(-85);
105 |     mpu.setZAccelOffset(1788);
106 | 
107 |     if (devStatus == 0) {
108 |         // Calibration Time
109 |         mpu.CalibrateAccel(6);
110 |         mpu.CalibrateGyro(6);
111 |         mpu.PrintActiveOffsets();
112 | 
113 |         // Enable DMP so the IMU fuses a quaternion for us
114 |         mpu.setDMPEnabled(true);
115 | 
116 |         // Setup interrupt
117 |         attachInterrupt(digitalPinToInterrupt(PIN_MPU6050_INT), dmpDataReady, RISING);
118 |         const uint8_t mpuIntStatus = mpu.getIntStatus();
119 | 
120 |         dmpReady = true;
121 | 
122 |         // Get expected DMP packet size for later comparison
123 |         packetSize = mpu.dmpGetFIFOPacketSize();
124 |     }
125 | }
126 | 
127 | void initRGB() {
128 |     rgb.begin();
129 |     rgb.clear();
130 | 
131 |     // Cycle colors for startup
132 |     for (const uint32_t color : {0x100000, 0x001000, 0x000010}) {
133 |         rgb.setPixelColor(0, color);
134 |         rgb.show();
135 |         delay(500);
136 |     }    
137 |     
138 |     rgb.clear();
139 |     rgb.show();
140 | }
141 | 
142 | void setup() {
143 |     // Initialize debug serial
144 |     Serial.begin(115200);
145 | 
146 |     // Initialize rpi UART
147 |     piUART.begin(230400, SERIAL_8N1, PIN_PI_UART_RX, PIN_PI_UART_TX);
148 | 
149 |     // Initialize ESC telemetry UART
150 |     if (ESC_TELEMETRY_ENABLED)
151 |         escTelemetryUART.begin(115200, SERIAL_8N1, PIN_ESC_UART_RX, PIN_ESC_UART_TX); 
152 | 
153 |     // Initialize servos
154 |     ESP32PWM::allocateTimer(0);
155 |     ESP32PWM::allocateTimer(1);
156 |     ESP32PWM::allocateTimer(2);
157 |     ESP32PWM::allocateTimer(3);
158 |     steeringServo.setPeriodHertz(50);
159 |     steeringServo.attach(PIN_STEERING_SERVO, servoMicrosMin, servoMicrosMax);
160 |     steeringServo.write(steeringServoDefaultMicros);
161 |     throttleServo.setPeriodHertz(50);
162 |     throttleServo.attach(PIN_THROTTLE_SERVO, servoMicrosMin, servoMicrosMax);
163 |     throttleServo.write(throttleServoDefaultMicros);
164 | 
165 |     // Initialize the RGB strip
166 |     initRGB();
167 | 
168 |     // Initialize I2C
169 |     Wire.begin();
170 |     Wire.setClock(400000);
171 | 
172 |     // Initialize MPU
173 |     initMPU6050();
174 | }
175 | 
176 | bool piMessageReceived(const uint8_t* payload, const uint16_t payloadSize) {
177 |     // Decode payload
178 |     if (payloadSize >= 1) {
179 |         const uint8_t commandId = payload[0];
180 | 
181 |         if (commandId == CMD_ECHO_REQUEST) {
182 |             // Send the payload back
183 |             uint8_t responsePayload[payloadSize];
184 |             memcpy(responsePayload, payload, payloadSize);
185 |             responsePayload[0] = CMD_ECHO_RESPONSE;
186 |             piMessaging.sendMessage(responsePayload, payloadSize);
187 |         }
188 | 
189 |         if (commandId == CMD_SET_RGB) {
190 |             for (uint8_t ledIndex = 0; ledIndex < NUM_LEDS; ledIndex++) {
191 |                 const uint16_t payloadIndex = (ledIndex * 4) + 1;
192 |                 if ((payloadIndex + 4) <= payloadSize) {
193 |                     const uint32_t color = *reinterpret_cast<const uint32_t*>(&payload[payloadIndex]);
194 |                     rgb.setPixelColor(ledIndex, color);
195 |                 }
196 |                 rgb.show();
197 |             }
198 |         }
199 |         if (commandId == CMD_SET_SERVOS) {
200 |             if (payloadSize >= 5) {
201 |                 const uint16_t parsedSteeringServoMicros = *reinterpret_cast<const uint16_t*>(&payload[1]);
202 |                 const uint16_t parsedThrottleServoMicros = *reinterpret_cast<const uint16_t*>(&payload[3]);
203 |                 
204 |                 // Update the servos
205 |                 steeringServoMicrosNew = parsedSteeringServoMicros;
206 |                 throttleServoMicrosNew = parsedThrottleServoMicros;
207 |                 newServoData = true;
208 | 
209 |                 // Keep track of when we last updated the servos
210 |                 lastSetServoMillis = millis();
211 |             }
212 |         }
213 |     }
214 | 
215 |     return true;
216 | }
217 | 
218 | void loop() {
219 |     piMessaging.update();
220 |     
221 |     uint32_t currentMillis = millis();
222 | 
223 |     // ESC Telemetry
224 |     if (ESC_TELEMETRY_ENABLED) {
225 |         if (escTelemetry.update()) {
226 |             uint8_t payload[1 + ESC_TELEMETRY_BUF_SIZE];
227 |             payload[0] = CMD_ESC_TELEMETRY_DUMP;
228 |             memcpy(&payload[1], escTelemetry.buffer, ESC_TELEMETRY_BUF_SIZE);
229 |             piMessaging.sendMessage(payload, 1 + ESC_TELEMETRY_BUF_SIZE);
230 |         }
231 |     }
232 | 
233 |     // MPU6050
234 |     if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) {
235 |         // Send the FIFO buffer
236 |         uint8_t payload[1 + packetSize];
237 |         payload[0] = CMD_IMU_DUMP;
238 |         memcpy(&payload[1], fifoBuffer, packetSize);
239 |         piMessaging.sendMessage(payload, 1 + packetSize);        
240 |     }
241 | 
242 |     // If we havent received a servo update in maxSetServoIntervalMillis, set the servos back to their defaults (failsafe)
243 |     if ((currentMillis - lastSetServoMillis) > maxSetServoIntervalMillis) {
244 |         steeringServo.write(steeringServoDefaultMicros);
245 |         throttleServo.write(throttleServoDefaultMicros);
246 |     } else if (newServoData) {
247 |         steeringServo.write(steeringServoMicrosNew);
248 |         throttleServo.write(throttleServoMicrosNew);
249 |     }
250 | }
251 | 


--------------------------------------------------------------------------------
/source/maus_board.cpp:
--------------------------------------------------------------------------------
  1 | #include "maus_board.h"
  2 | 
  3 | const uint8_t MausBoard::crcTable[] = {
  4 |     0x00, 0x31, 0x62, 0x53, 0xc4, 0xf5, 0xa6, 0x97, 0xb9, 0x88, 0xdb, 0xea, 0x7d, 0x4c, 0x1f, 0x2e, 
  5 |     0x43, 0x72, 0x21, 0x10, 0x87, 0xb6, 0xe5, 0xd4, 0xfa, 0xcb, 0x98, 0xa9, 0x3e, 0x0f, 0x5c, 0x6d, 
  6 |     0x86, 0xb7, 0xe4, 0xd5, 0x42, 0x73, 0x20, 0x11, 0x3f, 0x0e, 0x5d, 0x6c, 0xfb, 0xca, 0x99, 0xa8, 
  7 |     0xc5, 0xf4, 0xa7, 0x96, 0x01, 0x30, 0x63, 0x52, 0x7c, 0x4d, 0x1e, 0x2f, 0xb8, 0x89, 0xda, 0xeb, 
  8 |     0x3d, 0x0c, 0x5f, 0x6e, 0xf9, 0xc8, 0x9b, 0xaa, 0x84, 0xb5, 0xe6, 0xd7, 0x40, 0x71, 0x22, 0x13, 
  9 |     0x7e, 0x4f, 0x1c, 0x2d, 0xba, 0x8b, 0xd8, 0xe9, 0xc7, 0xf6, 0xa5, 0x94, 0x03, 0x32, 0x61, 0x50, 
 10 |     0xbb, 0x8a, 0xd9, 0xe8, 0x7f, 0x4e, 0x1d, 0x2c, 0x02, 0x33, 0x60, 0x51, 0xc6, 0xf7, 0xa4, 0x95, 
 11 |     0xf8, 0xc9, 0x9a, 0xab, 0x3c, 0x0d, 0x5e, 0x6f, 0x41, 0x70, 0x23, 0x12, 0x85, 0xb4, 0xe7, 0xd6, 
 12 |     0x7a, 0x4b, 0x18, 0x29, 0xbe, 0x8f, 0xdc, 0xed, 0xc3, 0xf2, 0xa1, 0x90, 0x07, 0x36, 0x65, 0x54, 
 13 |     0x39, 0x08, 0x5b, 0x6a, 0xfd, 0xcc, 0x9f, 0xae, 0x80, 0xb1, 0xe2, 0xd3, 0x44, 0x75, 0x26, 0x17, 
 14 |     0xfc, 0xcd, 0x9e, 0xaf, 0x38, 0x09, 0x5a, 0x6b, 0x45, 0x74, 0x27, 0x16, 0x81, 0xb0, 0xe3, 0xd2, 
 15 |     0xbf, 0x8e, 0xdd, 0xec, 0x7b, 0x4a, 0x19, 0x28, 0x06, 0x37, 0x64, 0x55, 0xc2, 0xf3, 0xa0, 0x91, 
 16 |     0x47, 0x76, 0x25, 0x14, 0x83, 0xb2, 0xe1, 0xd0, 0xfe, 0xcf, 0x9c, 0xad, 0x3a, 0x0b, 0x58, 0x69, 
 17 |     0x04, 0x35, 0x66, 0x57, 0xc0, 0xf1, 0xa2, 0x93, 0xbd, 0x8c, 0xdf, 0xee, 0x79, 0x48, 0x1b, 0x2a, 
 18 |     0xc1, 0xf0, 0xa3, 0x92, 0x05, 0x34, 0x67, 0x56, 0x78, 0x49, 0x1a, 0x2b, 0xbc, 0x8d, 0xde, 0xef, 
 19 |     0x82, 0xb3, 0xe0, 0xd1, 0x46, 0x77, 0x24, 0x15, 0x3b, 0x0a, 0x59, 0x68, 0xff, 0xce, 0x9d, 0xac
 20 | };
 21 | 
 22 | uint8_t MausBoard::calCRC8(const uint8_t *p, const size_t len) {
 23 |     uint8_t crc = 0;
 24 |     for (size_t i = 0; i < len; i++)
 25 |         crc = crcTable[(crc ^ p[i]) & 0xff];
 26 |     return crc;
 27 | }
 28 | 
 29 | float MausBoard::ImuData::getYawRadians() const {
 30 |     const float siny_cosp = 2 * (qW * qZ + qX * qY);
 31 |     const float cosy_cosp = 1 - 2 * (qY * qY + qZ * qZ);
 32 |     return std::atan2(siny_cosp, cosy_cosp);
 33 | }
 34 | 
 35 | MausBoard::ImuData MausBoard::ImuData::fromFifoPacket(const uint8_t* fifoPacket, const uint8_t fifoPacketSize) {
 36 |     ImuData imuData;
 37 | 
 38 |     if (fifoPacketSize >= 42) {
 39 |         // Parse quaternion
 40 |         imuData.qW = (float)((fifoPacket[0] << 8) | fifoPacket[1]) / 16384.0f;
 41 |         imuData.qX = (float)((fifoPacket[4] << 8) | fifoPacket[5]) / 16384.0f;
 42 |         imuData.qY = (float)((fifoPacket[8] << 8) | fifoPacket[9]) / 16384.0f;
 43 |         imuData.qZ = (float)((fifoPacket[12] << 8) | fifoPacket[13]) / 16384.0f;
 44 | 
 45 |         // Parse gryo
 46 |         imuData.gyroX = (float)((fifoPacket[16] << 8) | fifoPacket[17]);
 47 |         imuData.gyroY = (float)((fifoPacket[20] << 8) | fifoPacket[21]);
 48 |         imuData.gyroZ = (float)((fifoPacket[24] << 8) | fifoPacket[25]);
 49 | 
 50 |         // Parse accel
 51 |         imuData.accelX = (float)((fifoPacket[28] << 8) | fifoPacket[29]);
 52 |         imuData.accelY = (float)((fifoPacket[32] << 8) | fifoPacket[33]);
 53 |         imuData.accelZ = (float)((fifoPacket[36] << 8) | fifoPacket[37]);
 54 |     }
 55 | 
 56 |     return imuData;
 57 | }
 58 | 
 59 | void MausBoard::ImuData::writeBytes(std::ofstream& of) const {
 60 |     of.write((char*)&timestamp, sizeof(uint64_t));
 61 |     of.write((char*)&qX, sizeof(float));
 62 |     of.write((char*)&qY, sizeof(float));
 63 |     of.write((char*)&qZ, sizeof(float));
 64 |     of.write((char*)&qW, sizeof(float));
 65 |     of.write((char*)&gyroX, sizeof(float));
 66 |     of.write((char*)&gyroY, sizeof(float));
 67 |     of.write((char*)&gyroZ, sizeof(float));
 68 |     of.write((char*)&accelX, sizeof(float));
 69 |     of.write((char*)&accelY, sizeof(float));
 70 |     of.write((char*)&accelZ, sizeof(float));
 71 | }
 72 | 
 73 | MausBoard::ImuData MausBoard::ImuData::fromBytes(const char* bytes) {
 74 |     ImuData parsedImuData;
 75 |     parsedImuData.timestamp = *(uint64_t*)&bytes[0];
 76 |     parsedImuData.qX = *(float*)&bytes[sizeof(uint64_t)];
 77 |     parsedImuData.qY = *(float*)&bytes[sizeof(uint64_t) + 4];
 78 |     parsedImuData.qZ = *(float*)&bytes[sizeof(uint64_t) + 8];
 79 |     parsedImuData.qW = *(float*)&bytes[sizeof(uint64_t) + 12];
 80 |     parsedImuData.gyroX = *(float*)&bytes[sizeof(uint64_t) + 16];
 81 |     parsedImuData.gyroY = *(float*)&bytes[sizeof(uint64_t) + 20];
 82 |     parsedImuData.gyroZ = *(float*)&bytes[sizeof(uint64_t) + 24];
 83 |     parsedImuData.accelX = *(float*)&bytes[sizeof(uint64_t) + 28];
 84 |     parsedImuData.accelY = *(float*)&bytes[sizeof(uint64_t) + 32];
 85 |     parsedImuData.accelZ = *(float*)&bytes[sizeof(uint64_t) + 36];
 86 |     return parsedImuData;
 87 | }
 88 | 
 89 | MausBoard::EscTelemetry MausBoard::EscTelemetry::fromRawData(const uint8_t* rawData, const uint8_t rawDataSize) {
 90 |     EscTelemetry escTelemetry;
 91 | 
 92 |     if (rawDataSize >= 10) {
 93 |         escTelemetry.temperature = rawData[0];
 94 |         escTelemetry.voltage = ((uint16_t)(rawData[1]) << 8) | rawData[2];
 95 |         escTelemetry.current = ((uint16_t)(rawData[3]) << 8) | rawData[4];
 96 |         escTelemetry.consumption = ((uint16_t)(rawData[5]) << 8) | rawData[6];
 97 |         escTelemetry.ERPM = ((uint16_t)(rawData[7]) << 8) | rawData[8];
 98 |         // CRC not parsed
 99 |     }
100 | 
101 |     return escTelemetry;
102 | }
103 | 
104 | void MausBoard::parsePayload(const uint8_t* payload, const uint8_t payloadSize) {
105 |     if (payloadSize >= 1) {
106 |         const uint8_t commandId = payload[0];
107 | 
108 |         if (commandId == CommandIds::CMD_ECHO_REQUEST) {
109 |             // Send the payload back
110 |             uint8_t responsePayload[payloadSize];
111 |             memcpy(responsePayload, payload, payloadSize);
112 |             responsePayload[0] = CommandIds::CMD_ECHO_RESPONSE;
113 |             sendMessage(responsePayload, payloadSize);
114 |         }
115 | 
116 |         if (commandId == CommandIds::CMD_ECHO_RESPONSE) {
117 |             if (echoResponseCallback)
118 |                 echoResponseCallback(payload, payloadSize);
119 |         }
120 | 
121 |         if (commandId == CommandIds::CMD_IMU_DUMP) {
122 |             // Parse the data and call the callback
123 |             ImuData imuData = ImuData::fromFifoPacket(&payload[1], payloadSize - 1);
124 |             imuData.timestamp = TimeStamp::get();
125 |             imuDataCallback(imuData);
126 |         }
127 | 
128 |         if (commandId == CommandIds::CMD_ESC_TELEMETRY_DUMP) {
129 |             // Parse the data and call the callback
130 |             EscTelemetry escTelemetry = EscTelemetry::fromRawData(&payload[1], payloadSize - 1);
131 |             escTelemetry.timestamp = TimeStamp::get();
132 |             escTelemetryCallback(escTelemetry);
133 |         }
134 |     }
135 | }
136 | 
137 | void MausBoard::parseMessageBuffer() {
138 |     // Offset relative to messageBufferPos
139 |     uint16_t offset = 0;
140 | 
141 |     // Positions within magic bytes
142 |     uint8_t magicPosMessage = 0;
143 |     uint8_t magicPosAck = 0;
144 | 
145 |     while (offset < MAX_MESSAGE_SIZE) {
146 |         // Get the actual possition in messageBufferPos
147 |         const uint16_t bufferPos = (offset + messageBufferPos) % MAX_MESSAGE_SIZE;
148 |         offset++;
149 | 
150 |         magicPosMessage = (magicBytesMessage[magicPosMessage] == messageBuffer[bufferPos]) ? magicPosMessage + 1 : 0;
151 | 
152 |         // Check if we have magic bytes for a regular message
153 |         if (magicPosMessage == 2) {
154 |             magicPosMessage = 0;
155 |             // 2 byte magic number
156 |             // 1 byte message ID
157 |             // 1 byte payload size
158 |             // 1 byte payload CRC8
159 |             
160 |             // Check if there is enough room to read up to the CRC
161 |             if (offset + 3 <= MAX_MESSAGE_SIZE) {
162 |                 const uint8_t messageId = messageBuffer[(bufferPos + 1) % MAX_MESSAGE_SIZE];
163 |                 const uint8_t payloadSize = messageBuffer[(bufferPos + 2) % MAX_MESSAGE_SIZE];
164 |                 const uint8_t payloadCrc8 = messageBuffer[(bufferPos + 3) % MAX_MESSAGE_SIZE];
165 | 
166 |                 // Check if there is enough room to read the whole message
167 |                 if ((offset + 3 + payloadSize) <= MAX_MESSAGE_SIZE) {
168 |                     // Extract the payload
169 |                     uint8_t payload[255];
170 |                     for (uint16_t payloadIndex = 0; payloadIndex < payloadSize; payloadIndex++) {
171 |                         const uint16_t payloadBufferPos = (bufferPos + 4 + payloadIndex) % MAX_MESSAGE_SIZE;
172 |                         payload[payloadIndex] = messageBuffer[payloadBufferPos];
173 |                     }
174 | 
175 |                     // Check the CRC8
176 |                     const uint8_t payloadCrc8Calc = calCRC8(payload, payloadSize);
177 |                     if (payloadCrc8Calc == payloadCrc8) {
178 |                         // We have a message!
179 |                         // TODO do this in a thread because parsePayload can block for a while (it will screw up timestamping)
180 |                         parsePayload(payload, payloadSize);
181 |                         
182 |                         // TODO send an ack
183 | 
184 |                         // Zero out the message so we dont parse it again later
185 |                         for (uint16_t messageIndex = 0; messageIndex < (payloadSize + 5); messageIndex++) {
186 |                             const uint16_t zeroBufferPos = (bufferPos - 1 + messageIndex) % MAX_MESSAGE_SIZE;
187 |                             messageBuffer[zeroBufferPos] = 0x00;
188 |                         }
189 | 
190 |                         // Advance the offset
191 |                         offset += (payloadSize + 4);
192 |                     }
193 |                 }
194 |             }
195 |         }
196 |     }
197 | }
198 | 
199 | void MausBoard::readLoop() {
200 |     // Read forever
201 |     if (uartFileStream != -1) {
202 |         uint8_t uartBuffer[UART_BUFFER_SIZE];
203 |         while (readingUart) {
204 |             // Read and parse data
205 |             int len = read(uartFileStream, uartBuffer, UART_BUFFER_SIZE);
206 |             if (len > 0) {
207 |                 for (int16_t i = 0; i < len; i++) {
208 |                     messageBuffer[messageBufferPos] = uartBuffer[i];
209 |                     messageBufferPos = (messageBufferPos + 1) % MAX_MESSAGE_SIZE;
210 |                 }
211 | 
212 |                 parseMessageBuffer();
213 |             }
214 |         }
215 |     }
216 | 
217 |     // Close the UART
218 |     close(uartFileStream);
219 | }
220 | 
221 | void MausBoard::sendMessage(const uint8_t* payload, const uint8_t payloadSize) {
222 |     if (uartFileStream != -1) {
223 |         // Build header
224 |         uint8_t header[5];
225 |         memcpy(header, magicBytesMessage, 2);
226 |         header[2] = 0;
227 |         header[3] = payloadSize;
228 |         header[4] = calCRC8(payload, payloadSize);
229 | 
230 |         // Write the header and payload
231 |         write(uartFileStream, header, 5);
232 |         write(uartFileStream, payload, payloadSize);
233 |     }
234 | }
235 | 
236 | bool MausBoard::startReading() {
237 |     if (!readingUart) {
238 |         // Open the UART
239 |         uartFileStream = open(DEFAULT_SERIAL_MAUS_BOARD, O_RDWR);
240 |         if (uartFileStream == -1) {
241 |             printf("Unable to open UART\n");
242 |             return false;
243 |         }
244 | 
245 |         // Configure the UART (Flags defined in /usr/include/termios.h - see http://pubs.opengroup.org/onlinepubs/007908799/xsh/termios.h.html)
246 |         struct termios options;
247 |         tcgetattr(uartFileStream, &options);
248 |         options.c_cflag = B230400 | CS8 | CLOCAL | CREAD; // Set baud rate
249 |         options.c_iflag = IGNPAR;
250 |         options.c_oflag = 0;
251 |         options.c_lflag = 0;
252 |         tcflush(uartFileStream, TCIFLUSH);
253 |         tcsetattr(uartFileStream, TCSANOW, &options);
254 | 
255 |         readingUart = true;
256 |         readingThread = std::thread(&MausBoard::readLoop, this);
257 | 
258 |         return true;
259 |     } else {
260 |         printf("Cannot start reading. UART is already being read\n");
261 |         return false;
262 |     }
263 | }
264 | 
265 | bool MausBoard::stopReading() {
266 |     if (readingUart) {
267 |         readingUart = false;
268 |         readingThread.join();
269 |         uartFileStream = -1;
270 | 
271 |         return true;
272 |     }
273 |     return false;
274 | }
275 | 
276 | void MausBoard::sendSetServos(const uint16_t steering, const uint16_t throttle) {
277 |     // Build the payload
278 |     uint8_t payload[1 + 4];
279 |     payload[0] = CommandIds::CMD_SET_SERVOS;
280 |     memcpy(&payload[1], &steering, 2);
281 |     memcpy(&payload[3], &throttle, 2);
282 | 
283 |     // Send it
284 |     sendMessage(payload, 1 + 4);
285 | }
286 | 
287 | void MausBoard::sentSetRGB(const std::vector<uint32_t>& colors) {
288 |     // Build the payload 
289 |     const uint8_t payloadSize = 1 + (colors.size() * 4);
290 |     uint8_t payload[payloadSize];
291 |     payload[0] = CommandIds::CMD_SET_RGB;
292 |     for (size_t i = 0; i < colors.size(); i++) {
293 |         const uint8_t payloadIndex = 1 + (i * 4);
294 |         memcpy(&payload[payloadIndex], &colors[i], 4);
295 |     }
296 | 
297 |     // Send it
298 |     sendMessage(payload, payloadSize);
299 | }
300 | 
301 | void MausBoard::sendEcho(const uint8_t* data, const uint8_t dataSize) {
302 |     // Build the payload
303 |     const uint8_t payloadSize = 1 + dataSize;
304 |     uint8_t payload[payloadSize];
305 |     payload[0] = CommandIds::CMD_ECHO_REQUEST;
306 |     memcpy(&payload[1], data, dataSize);
307 | 
308 |     // Send it
309 |     sendMessage(payload, payloadSize);
310 | }


--------------------------------------------------------------------------------