├── .gitignore ├── birdeye ├── pic3.jpg ├── CMakeLists.txt └── birdeye.cpp ├── drive ├── Navio │ ├── Util.h │ ├── RCInput.h │ ├── ADC.h │ ├── PWM.h │ ├── RGBled.h │ ├── InertialSensor.h │ ├── RCInput.cpp │ ├── ADC.cpp │ ├── Util.cpp │ ├── RGBled.cpp │ ├── PWM.cpp │ ├── gpio.h │ ├── MB85RC04.h │ ├── MB85RC256.h │ ├── MB85RC04.cpp │ ├── MB85RC256.cpp │ ├── SPIdev.h │ ├── MS5611.h │ ├── PCA9685.h │ ├── Ublox.h │ ├── gpio.cpp │ ├── I2Cdev.h │ ├── MS5611.cpp │ ├── LSM9DS1.h │ ├── PCA9685.cpp │ ├── ADS1115.h │ ├── LSM9DS1.cpp │ ├── ADS1115.cpp │ ├── MPU9250.h │ ├── MPU9250.cpp │ ├── I2Cdev.cpp │ └── Ublox.cpp ├── Radio.h ├── Motor.h ├── CMakeLists.txt ├── Radio.cpp ├── Motor.cpp └── main.cpp ├── think ├── Timer.h ├── ImageLogger.h ├── Capture.h ├── VideoSource.h ├── Compute.h ├── Timer.cpp ├── VideoSource.cpp ├── ImageLogger.cpp ├── queue.h ├── Capture.cpp └── Compute.cpp ├── look ├── ImageView.h ├── CMakeLists.txt ├── ImageView.cpp └── main.cpp ├── LICENSE └── README.md /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *.user 3 | -------------------------------------------------------------------------------- /birdeye/pic3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/compound-eye/windshield/HEAD/birdeye/pic3.jpg -------------------------------------------------------------------------------- /drive/Navio/Util.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #define ARRAY_SIZE(a) sizeof(a) / sizeof(a[0]) 4 | 5 | int write_file(const char *path, const char *fmt, ...); 6 | int read_file(const char *path, const char *fmt, ...); 7 | bool check_apm(); 8 | -------------------------------------------------------------------------------- /birdeye/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(birdeye) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | find_package(OpenCV REQUIRED) 5 | 6 | aux_source_directory(. SRC_LIST) 7 | add_executable(${PROJECT_NAME} ${SRC_LIST}) 8 | 9 | target_link_libraries(${PROJECT_NAME} ${OpenCV_LIBS}) 10 | -------------------------------------------------------------------------------- /drive/Radio.h: -------------------------------------------------------------------------------- 1 | #ifndef RADIO_H 2 | #define RADIO_H 3 | 4 | #include "Navio/RCInput.h" 5 | 6 | 7 | class Radio { 8 | public: 9 | Radio(); 10 | 11 | float ReadThrottle(); 12 | float ReadSteer(); 13 | 14 | private: 15 | RCInput rc; 16 | }; 17 | 18 | #endif // RADIO_H 19 | -------------------------------------------------------------------------------- /drive/Motor.h: -------------------------------------------------------------------------------- 1 | #ifndef MOTOR_H 2 | #define MOTOR_H 3 | 4 | #include "Navio/PWM.h" 5 | 6 | 7 | class Motor { 8 | public: 9 | Motor(); 10 | 11 | void SetLeftMotor (float v); 12 | void SetRightMotor(float v); 13 | 14 | private: 15 | PWM pwm; 16 | }; 17 | 18 | #endif // MOTOR_H 19 | -------------------------------------------------------------------------------- /think/Timer.h: -------------------------------------------------------------------------------- 1 | #ifndef TIMER_H 2 | #define TIMER_H 3 | 4 | #include 5 | 6 | 7 | class Timer { 8 | public: 9 | void Start(); 10 | int NextSleep(int freq, int frameCount); 11 | 12 | void PrintTimeStats(int frameCount); 13 | 14 | private: 15 | struct timeval startTime; 16 | }; 17 | 18 | #endif // TIMER_H 19 | -------------------------------------------------------------------------------- /drive/Navio/RCInput.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class RCInput 6 | { 7 | public: 8 | void init(); 9 | int read(int ch); 10 | RCInput(); 11 | ~RCInput(); 12 | 13 | private: 14 | int open_channel(int ch); 15 | 16 | static const size_t CHANNEL_COUNT = 14; 17 | int channels[CHANNEL_COUNT]; 18 | }; 19 | -------------------------------------------------------------------------------- /drive/Navio/ADC.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | 3 | #include 4 | 5 | class ADC 6 | { 7 | public: 8 | void init(); 9 | int read(int ch); 10 | int get_channel_count(void); 11 | ADC(); 12 | ~ADC(); 13 | 14 | private: 15 | int open_channel(int ch); 16 | 17 | static const size_t CHANNEL_COUNT = 6; 18 | int channels[CHANNEL_COUNT]; 19 | }; 20 | -------------------------------------------------------------------------------- /think/ImageLogger.h: -------------------------------------------------------------------------------- 1 | #ifndef IMAGELOGGER_H 2 | #define IMAGELOGGER_H 3 | 4 | #include "queue.h" 5 | #include 6 | 7 | 8 | class ImageLogger { 9 | public: 10 | Queue imagesToLog; 11 | 12 | void Start(); 13 | void Stop(); 14 | void BackgroundLoop(); 15 | 16 | private: 17 | pthread_t thread; 18 | }; 19 | 20 | #endif // IMAGELOGGER_H 21 | -------------------------------------------------------------------------------- /drive/Navio/PWM.h: -------------------------------------------------------------------------------- 1 | #ifndef _PWD_H_ 2 | #define _PWD_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | class PWM { 10 | public: 11 | PWM(); 12 | 13 | bool init(unsigned int channel); 14 | bool enable(unsigned int channel); 15 | bool set_period(unsigned int channel, unsigned int freq); 16 | bool set_duty_cycle(unsigned int channel, float period); 17 | }; 18 | 19 | #endif //_PWD_H_ 20 | 21 | -------------------------------------------------------------------------------- /look/ImageView.h: -------------------------------------------------------------------------------- 1 | #ifndef IMAGEVIEW_H 2 | #define IMAGEVIEW_H 3 | 4 | #include 5 | 6 | 7 | struct Mouse { 8 | int x0, y0, x1, y1; 9 | bool down; 10 | }; 11 | 12 | class OutputData; 13 | 14 | class ImageView { 15 | public: 16 | ImageView(int imageWidth, int imageHeight); 17 | ~ImageView(); 18 | 19 | void Draw(const OutputData& data, int viewWidth, int viewHeight, const Mouse& mouse) const; 20 | 21 | private: 22 | GLuint tex; 23 | }; 24 | 25 | #endif // IMAGEVIEW_H 26 | -------------------------------------------------------------------------------- /think/Capture.h: -------------------------------------------------------------------------------- 1 | #ifndef CAPTURE_H 2 | #define CAPTURE_H 3 | 4 | #include "VideoSource.h" 5 | #include 6 | 7 | 8 | class Capture : public VideoSource { 9 | public: 10 | Capture(int device); 11 | Capture(const cv::String& filename, int api); 12 | 13 | void Start(); 14 | void Stop(); 15 | 16 | void BackgroundLoop(); 17 | 18 | private: 19 | pthread_t thread; 20 | cv::VideoCapture cap; 21 | const int fps; 22 | bool canDropFrame; 23 | }; 24 | 25 | #endif // CAPTURE_H 26 | -------------------------------------------------------------------------------- /drive/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(rover) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | find_package(OpenCV REQUIRED) 5 | 6 | include_directories(../think) 7 | 8 | aux_source_directory(. SRC_LIST) 9 | aux_source_directory(../think SRC_LIST) 10 | 11 | add_executable(${PROJECT_NAME} 12 | ${SRC_LIST} 13 | ../think/queue.h 14 | Navio/PWM.cpp 15 | Navio/RCInput.cpp 16 | Navio/Util.cpp) 17 | 18 | target_link_libraries(${PROJECT_NAME} 19 | pthread 20 | ${OpenCV_LIBS}) 21 | -------------------------------------------------------------------------------- /drive/Navio/RGBled.h: -------------------------------------------------------------------------------- 1 | #ifndef _RGBLED_H_ 2 | #define _RGBLED_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include "gpio.h" 8 | 9 | namespace Colors { 10 | enum T { 11 | Black, 12 | Red, 13 | Green, 14 | Blue, 15 | Cyan, 16 | Magenta, 17 | Yellow, 18 | White 19 | }; 20 | } 21 | 22 | class RGBled { 23 | public: 24 | RGBled(); 25 | 26 | bool initialize(); 27 | void setColor(Colors::T color); 28 | 29 | private: 30 | Navio::Pin *pinR; 31 | Navio::Pin *pinG; 32 | Navio::Pin *pinB; 33 | }; 34 | 35 | #endif //_RGBLED_H_ 36 | -------------------------------------------------------------------------------- /think/VideoSource.h: -------------------------------------------------------------------------------- 1 | #ifndef VIDEOSOURCE_H 2 | #define VIDEOSOURCE_H 3 | 4 | #include "queue.h" 5 | #include 6 | 7 | 8 | class Timer; 9 | 10 | class VideoSource { 11 | public: 12 | enum Command {Noop, Play, Pause, Rewind, NextFrame, PrevFrame}; 13 | 14 | int imageWidth, imageHeight; 15 | Queue imagesCaptured; 16 | Queue commands; 17 | bool printTimeStats; 18 | bool playing; 19 | 20 | virtual void Start() = 0; 21 | virtual void Stop() = 0; 22 | virtual ~VideoSource(); 23 | 24 | Command NextCommand(int& frameCount, Timer& timer); 25 | void PrintTimeStats(int frameCount, Timer& timer); 26 | }; 27 | 28 | #endif // VIDEOSOURCE_H 29 | -------------------------------------------------------------------------------- /look/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | project(windshield) 2 | cmake_minimum_required(VERSION 2.8) 3 | 4 | find_package(OpenCV REQUIRED) 5 | find_package(OpenGL REQUIRED) 6 | find_package(GLUT REQUIRED) 7 | find_package(GLEW REQUIRED) 8 | 9 | include_directories(../think) 10 | 11 | aux_source_directory(. SRC_LIST) 12 | aux_source_directory(../think SRC_LIST) 13 | 14 | add_executable(${PROJECT_NAME} 15 | ${SRC_LIST} 16 | ../think/queue.h) 17 | 18 | target_link_libraries(${PROJECT_NAME} 19 | pthread 20 | ${OpenCV_LIBS} 21 | ${OPENGL_LIBRARIES} 22 | ${GLUT_glut_LIBRARY} 23 | ${GLEW_LIBRARIES}) 24 | -------------------------------------------------------------------------------- /drive/Navio/InertialSensor.h: -------------------------------------------------------------------------------- 1 | #ifndef _INERTIAL_SENSOR_H 2 | #define _INERTIAL_SENSOR_H 3 | 4 | class InertialSensor { 5 | public: 6 | virtual bool initialize() = 0; 7 | virtual bool probe() = 0; 8 | virtual void update() = 0; 9 | 10 | float read_temperature() {return temperature;}; 11 | void read_accelerometer(float *ax, float *ay, float *az) {*ax = _ax; *ay = _ay; *az = _az;}; 12 | void read_gyroscope(float *gx, float *gy, float *gz) {*gx = _gx; *gy = _gy; *gz = _gz;}; 13 | void read_magnetometer(float *mx, float *my, float *mz) {*mx = _mx; *my = _my; *mz = _mz;}; 14 | 15 | protected: 16 | float temperature; 17 | float _ax, _ay, _az; 18 | float _gx, _gy, _gz; 19 | float _mx, _my, _mz; 20 | }; 21 | 22 | #endif //_INERTIAL_SENSOR_H 23 | -------------------------------------------------------------------------------- /think/Compute.h: -------------------------------------------------------------------------------- 1 | #ifndef COMPUTE_H 2 | #define COMPUTE_H 3 | 4 | #include 5 | 6 | class ImageLogger; 7 | class VideoSource; 8 | 9 | 10 | typedef std::vector Lines; 11 | 12 | enum Direction {GoStraight, Turn, GoBack}; 13 | 14 | struct OutputData { 15 | cv::Mat imageBefore, imageAfter; 16 | Lines lines; 17 | float angle; 18 | int countFramesWithoutLines; 19 | }; 20 | 21 | class Compute { 22 | public: 23 | Compute(VideoSource* c, ImageLogger* lg, bool outPic); 24 | ~Compute(); 25 | 26 | void Start(); 27 | void Stop(); 28 | void BackgroundLoop(); 29 | 30 | void SwapOutputData(OutputData& data); 31 | 32 | private: 33 | VideoSource* cap; 34 | ImageLogger* log; 35 | bool outputPic; 36 | int countFramesWithoutLines; 37 | OutputData outputData; 38 | pthread_mutex_t dataMutex; 39 | pthread_t thread; 40 | }; 41 | 42 | #endif // COMPUTE_H 43 | -------------------------------------------------------------------------------- /think/Timer.cpp: -------------------------------------------------------------------------------- 1 | #include "Timer.h" 2 | #include 3 | 4 | 5 | void Timer::Start() { 6 | gettimeofday(&startTime, 0); 7 | } 8 | 9 | int Timer::NextSleep(int freq, int frameCount) { 10 | int period = 1000000 / freq; 11 | struct timeval now; 12 | if (gettimeofday(&now, 0) == 0) { 13 | int elapsed = 1000000 * (now.tv_sec - startTime.tv_sec) + now.tv_usec - startTime.tv_usec; 14 | int cur = elapsed / period; 15 | int sleep = cur < frameCount ? (cur + 1) * period - elapsed : 0; 16 | return sleep; 17 | } else { 18 | return period; 19 | } 20 | } 21 | 22 | void Timer::PrintTimeStats(int frameCount) { 23 | struct timeval now; 24 | if (gettimeofday(&now, 0) == 0) { 25 | double elapsed = now.tv_sec - startTime.tv_sec + (now.tv_usec - startTime.tv_usec)/1e6; 26 | std::cerr << frameCount << " frames in " << elapsed << " seconds; fps = " << frameCount / elapsed << std::endl; 27 | } 28 | } 29 | -------------------------------------------------------------------------------- /think/VideoSource.cpp: -------------------------------------------------------------------------------- 1 | #include "VideoSource.h" 2 | #include "Timer.h" 3 | 4 | 5 | VideoSource::~VideoSource() {} 6 | 7 | VideoSource::Command VideoSource::NextCommand(int& frameCount, Timer& timer) { 8 | if (playing && commands.size <= 0) { 9 | return Noop; 10 | } else { 11 | Command cmd = commands.Dequeue(); 12 | switch (cmd) { 13 | 14 | case Play: 15 | case Rewind: 16 | playing = true; 17 | frameCount = 0; 18 | timer.Start(); 19 | break; 20 | 21 | case Pause: 22 | case NextFrame: 23 | case PrevFrame: 24 | if (playing) { 25 | playing = false; 26 | PrintTimeStats(frameCount, timer); 27 | } 28 | break; 29 | } 30 | 31 | return cmd; 32 | } 33 | } 34 | 35 | void VideoSource::PrintTimeStats(int frameCount, Timer& timer) { 36 | if (printTimeStats) { 37 | timer.PrintTimeStats(frameCount); 38 | } 39 | } 40 | -------------------------------------------------------------------------------- /drive/Radio.cpp: -------------------------------------------------------------------------------- 1 | #include "Radio.h" 2 | #include 3 | 4 | 5 | static const int chThrottle = 0, minThrottle = 1120, maxThrottle = 1880; 6 | static const int chRoll = 1, rightRoll = 1110, leftRoll = 1890; 7 | static const int chPitch = 2, upPitch = 1110, downPitch = 1890; 8 | static const int chYaw = 3, rightYaw = 1110, leftYaw = 1890; 9 | 10 | static const int inpCenter = 1500; 11 | static const int inpCenterR = 5; 12 | 13 | Radio::Radio() { 14 | rc.init(); 15 | } 16 | 17 | float Radio::ReadThrottle() { 18 | return std::min(std::max(0.F, float(rc.read(chThrottle) - minThrottle) / (maxThrottle - minThrottle)), 1.F); 19 | } 20 | 21 | float Radio::ReadSteer() { 22 | int steer = rc.read(chRoll); 23 | if (steer > inpCenter + inpCenterR) { 24 | return std::min(1.F, float(steer - (inpCenter + inpCenterR)) / (leftRoll - (inpCenter + inpCenterR))); 25 | } else if (steer < inpCenter - inpCenterR) { 26 | return std::max(-1.F, float(steer - (inpCenter - inpCenterR)) / (inpCenter - inpCenterR - rightRoll)); 27 | } else { 28 | return 0.; 29 | } 30 | } 31 | -------------------------------------------------------------------------------- /drive/Navio/RCInput.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "RCInput.h" 8 | #include "Util.h" 9 | 10 | #define RCIN_SYSFS_PATH "/sys/kernel/rcio/rcin" 11 | 12 | RCInput::RCInput() 13 | { 14 | 15 | } 16 | 17 | RCInput::~RCInput() 18 | { 19 | } 20 | 21 | void RCInput::init() 22 | { 23 | for (size_t i = 0; i < ARRAY_SIZE(channels); i++) { 24 | channels[i] = open_channel(i); 25 | if (channels[i] < 0) { 26 | perror("open"); 27 | } 28 | } 29 | } 30 | 31 | int RCInput::read(int ch) 32 | { 33 | char buffer[10]; 34 | 35 | if (::pread(channels[ch], buffer, ARRAY_SIZE(buffer), 0) < 0) { 36 | perror("pread"); 37 | } 38 | 39 | return atoi(buffer); 40 | } 41 | 42 | int RCInput::open_channel(int channel) 43 | { 44 | char *channel_path; 45 | if (asprintf(&channel_path, "%s/ch%d", RCIN_SYSFS_PATH, channel) == -1) { 46 | err(1, "channel: %d\n", channel); 47 | } 48 | 49 | int fd = ::open(channel_path, O_RDONLY); 50 | 51 | free(channel_path); 52 | 53 | return fd; 54 | } 55 | 56 | -------------------------------------------------------------------------------- /think/ImageLogger.cpp: -------------------------------------------------------------------------------- 1 | #include "ImageLogger.h" 2 | #include 3 | #include 4 | #include 5 | 6 | 7 | void ImageLogger::BackgroundLoop() { 8 | char fname[100]; 9 | 10 | time_t now = time(NULL); 11 | strftime(fname, sizeof fname, "/%F-%H%M%S", localtime(&now)); 12 | 13 | static const char* pathLogHomeDir = "/home/pi/rover-images"; 14 | mkdir(pathLogHomeDir, ACCESSPERMS); 15 | std::string pathLogDir = std::string(pathLogHomeDir) + fname; 16 | mkdir(pathLogDir.c_str(), ACCESSPERMS); 17 | 18 | for (int i = 0; ! imagesToLog.quitting; ++i) { 19 | cv::Mat image = imagesToLog.Dequeue(); 20 | if (! image.empty()) { 21 | sprintf(fname, "%s/%04d.png", pathLogDir.c_str(), i); 22 | cv::imwrite(fname, image); 23 | } 24 | } 25 | } 26 | 27 | static void* ImageLoggerThread(void* logger) { 28 | ((ImageLogger*)logger)->BackgroundLoop(); 29 | return 0; 30 | } 31 | 32 | void ImageLogger::Start() { 33 | pthread_create(&thread, NULL, ImageLoggerThread, this); 34 | } 35 | 36 | void ImageLogger::Stop() { 37 | imagesToLog.Quit(); 38 | pthread_join(thread, NULL); 39 | } 40 | -------------------------------------------------------------------------------- /drive/Motor.cpp: -------------------------------------------------------------------------------- 1 | #include "Motor.h" 2 | 3 | 4 | static const int chTiltCamera = 0; 5 | static const int chLeftMotor = 1; 6 | static const int chRightMotor = 2; 7 | static const int chPanCamera = 3; 8 | 9 | static const float outMin = 1.; 10 | static const float outNeutral = 1.5; 11 | static const float outMax = 2.; 12 | 13 | static void InitPWMChannel(PWM& pwm, int ch) { 14 | pwm.init(ch); 15 | pwm.enable(ch); 16 | pwm.set_period(ch, 50); 17 | } 18 | 19 | Motor::Motor() { 20 | InitPWMChannel(pwm, chLeftMotor); 21 | InitPWMChannel(pwm, chRightMotor); 22 | InitPWMChannel(pwm, chTiltCamera); 23 | InitPWMChannel(pwm, chPanCamera); 24 | 25 | pwm.set_duty_cycle(chLeftMotor, outNeutral); 26 | pwm.set_duty_cycle(chRightMotor, outNeutral); 27 | pwm.set_duty_cycle(chPanCamera, outNeutral); 28 | pwm.set_duty_cycle(chTiltCamera, 1.); 29 | pwm.set_duty_cycle(chPanCamera, 1.47); 30 | } 31 | 32 | void Motor::SetLeftMotor(float v) { 33 | pwm.set_duty_cycle(chLeftMotor, v * (outMax - outNeutral) + outNeutral); 34 | } 35 | 36 | void Motor::SetRightMotor(float v) { 37 | pwm.set_duty_cycle(chRightMotor, v * (outMax - outNeutral) + outNeutral); 38 | } 39 | -------------------------------------------------------------------------------- /drive/Navio/ADC.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include "ADC.h" 8 | #include "Util.h" 9 | 10 | #define ADC_SYSFS_PATH "/sys/kernel/rcio/adc" 11 | 12 | ADC::ADC() 13 | { 14 | 15 | } 16 | 17 | ADC::~ADC() 18 | { 19 | } 20 | 21 | void ADC::init() 22 | { 23 | for (size_t i = 0; i < ARRAY_SIZE(channels); i++) { 24 | channels[i] = open_channel(i); 25 | if (channels[i] < 0) { 26 | perror("open"); 27 | } 28 | } 29 | } 30 | 31 | int ADC::get_channel_count(void) 32 | { 33 | return CHANNEL_COUNT; 34 | } 35 | 36 | int ADC::read(int ch) 37 | { 38 | char buffer[10]; 39 | 40 | if (::pread(channels[ch], buffer, ARRAY_SIZE(buffer), 0) < 0) { 41 | perror("pread"); 42 | } 43 | 44 | return atoi(buffer); 45 | } 46 | 47 | int ADC::open_channel(int channel) 48 | { 49 | char *channel_path; 50 | if (asprintf(&channel_path, "%s/ch%d", ADC_SYSFS_PATH, channel) == -1) { 51 | err(1, "adc channel: %d\n", channel); 52 | } 53 | 54 | int fd = ::open(channel_path, O_RDONLY); 55 | 56 | free(channel_path); 57 | 58 | return fd; 59 | } 60 | 61 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2017, Compound Eye 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /drive/Navio/Util.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | 8 | #include "Util.h" 9 | 10 | #define SCRIPT_PATH "../../../check_apm.sh" 11 | 12 | int write_file(const char *path, const char *fmt, ...) 13 | { 14 | errno = 0; 15 | 16 | int fd = ::open(path, O_WRONLY | O_CLOEXEC); 17 | if (fd == -1) { 18 | return -errno; 19 | } 20 | 21 | va_list args; 22 | va_start(args, fmt); 23 | 24 | int ret = ::vdprintf(fd, fmt, args); 25 | int errno_bkp = errno; 26 | ::close(fd); 27 | 28 | va_end(args); 29 | 30 | if (ret < 1) { 31 | return -errno_bkp; 32 | } 33 | 34 | return ret; 35 | } 36 | 37 | int read_file(const char *path, const char *fmt, ...) 38 | { 39 | errno = 0; 40 | 41 | FILE *file = ::fopen(path, "re"); 42 | if (!file) 43 | return -errno; 44 | 45 | va_list args; 46 | va_start(args, fmt); 47 | 48 | int ret = ::vfscanf(file, fmt, args); 49 | int errno_bkp = errno; 50 | ::fclose(file); 51 | 52 | va_end(args); 53 | 54 | if (ret < 1) 55 | return -errno_bkp; 56 | 57 | return ret; 58 | } 59 | 60 | bool check_apm() 61 | { 62 | int ret = system("ps -AT | grep -c sched-timer > /dev/null"); 63 | 64 | if (WEXITSTATUS(ret) <= 0) { 65 | fprintf(stderr, "APM is running. Can't launch the example\n"); 66 | return true; 67 | } 68 | 69 | return false; 70 | } 71 | -------------------------------------------------------------------------------- /drive/Navio/RGBled.cpp: -------------------------------------------------------------------------------- 1 | #include "RGBled.h" 2 | 3 | // Output is inverted 4 | #define OFF 1 5 | #define ON 0 6 | 7 | using namespace Navio; 8 | 9 | RGBled::RGBled() 10 | { 11 | } 12 | 13 | bool RGBled::initialize() { 14 | pinR = new Pin(RPI_GPIO_4); 15 | pinG = new Pin(RPI_GPIO_27); 16 | pinB = new Pin(RPI_GPIO_6); 17 | if (pinR->init() & pinG->init() & pinB->init()) { 18 | pinR->setMode(Pin::GpioModeOutput); 19 | pinG->setMode(Pin::GpioModeOutput); 20 | pinB->setMode(Pin::GpioModeOutput); 21 | 22 | // Switch of LED 23 | pinR->write(OFF); 24 | pinG->write(OFF); 25 | pinB->write(OFF); 26 | } 27 | else { 28 | fprintf(stderr, "Output enable not set. Are you root?\n"); 29 | return false; 30 | } 31 | return true; 32 | } 33 | 34 | void RGBled::setColor(Colors::T color) 35 | { 36 | switch (color) { 37 | case Colors::Black: pinR->write(OFF); pinG->write(OFF); pinB->write(OFF); break; 38 | case Colors::Red: pinR->write(ON); pinG->write(OFF); pinB->write(OFF); break; 39 | case Colors::Green: pinR->write(OFF); pinG->write(ON); pinB->write(OFF); break; 40 | case Colors::Blue: pinR->write(OFF); pinG->write(OFF); pinB->write(ON); break; 41 | case Colors::Cyan: pinR->write(OFF); pinG->write(ON); pinB->write(ON); break; 42 | case Colors::Magenta: pinR->write(ON); pinG->write(OFF); pinB->write(ON); break; 43 | case Colors::Yellow: pinR->write(ON); pinG->write(ON); pinB->write(OFF); break; 44 | case Colors::White: pinR->write(ON); pinG->write(ON); pinB->write(ON); break; 45 | } 46 | } 47 | -------------------------------------------------------------------------------- /drive/Navio/PWM.cpp: -------------------------------------------------------------------------------- 1 | #include "PWM.h" 2 | #include "Util.h" 3 | 4 | PWM::PWM() 5 | { 6 | } 7 | 8 | bool PWM::init(unsigned int channel) 9 | { 10 | int err; 11 | err = write_file("/sys/class/pwm/pwmchip0/export", "%u", channel); 12 | if (err >= 0 || err == -EBUSY) 13 | { 14 | return true; 15 | } 16 | else 17 | { 18 | printf("Can't init channel %u\n", channel); 19 | return false; 20 | } 21 | return true; 22 | } 23 | 24 | bool PWM::enable(unsigned int channel) 25 | { 26 | char path[60] = "/sys/class/pwm/pwmchip0"; 27 | char path_ch[20]; 28 | sprintf(path_ch, "/pwm%u/enable", channel); 29 | strcat(path, path_ch); 30 | 31 | if (write_file(path, "1") < 0) 32 | { 33 | printf("Can't enable channel %u\n", channel); 34 | return false; 35 | } 36 | return true; 37 | } 38 | 39 | bool PWM::set_period(unsigned int channel, unsigned int freq) 40 | { 41 | int period_ns; 42 | char path[60] = "/sys/class/pwm/pwmchip0"; 43 | char path_ch[20]; 44 | sprintf(path_ch, "/pwm%u/period", channel); 45 | strcat(path, path_ch); 46 | 47 | period_ns = 1e9 / freq; 48 | if (write_file(path, "%u", period_ns) < 0) 49 | { 50 | printf("Can't set period to channel %u\n", channel); 51 | return false; 52 | } 53 | return true; 54 | } 55 | 56 | bool PWM::set_duty_cycle(unsigned int channel, float period) 57 | { 58 | int period_ns; 59 | char path[60] = "/sys/class/pwm/pwmchip0"; 60 | char path_ch[20]; 61 | sprintf(path_ch, "/pwm%u/duty_cycle", channel); 62 | strcat(path, path_ch); 63 | 64 | period_ns = period * 1e6; 65 | if (write_file(path, "%u", period_ns) < 0) 66 | { 67 | printf("Can't set duty cycle to channel %u\n", channel); 68 | return false; 69 | } 70 | return true; 71 | } 72 | -------------------------------------------------------------------------------- /drive/Navio/gpio.h: -------------------------------------------------------------------------------- 1 | 2 | #ifndef __NAVIO_GPIO_H__ 3 | #define __NAVIO_GPIO_H__ 4 | 5 | #include 6 | 7 | namespace Navio { 8 | 9 | /* Raspberry Pi GPIO mapping */ 10 | #define RPI_GPIO_2 2 // Pin 3 SDA 11 | #define RPI_GPIO_3 3 // Pin 5 SCL 12 | #define RPI_GPIO_4 4 // Pin 7 NAVIO_PPM_INPUT 13 | #define RPI_GPIO_6 6 14 | #define RPI_GPIO_7 7 // Pin 26 CE1 NAVIO_MPU9250_CS 15 | #define RPI_GPIO_8 8 // Pin 24 CE0 NAVIO_UBLOX_CS 16 | #define RPI_GPIO_9 9 // Pin 21 MISO 17 | #define RPI_GPIO_10 10 // Pin 19 MOSI 18 | #define RPI_GPIO_11 11 // Pin 23 SCLK 19 | #define RPI_GPIO_14 14 // Pin 8 TxD 20 | #define RPI_GPIO_15 15 // Pin 10 RxD 21 | #define RPI_GPIO_17 17 // Pin 11 NAVIO_UART_PORT_5 22 | #define RPI_GPIO_18 18 // Pin 12 NAVIO_UART_PORT_4 23 | #define RPI_GPIO_22 22 // Pin 15 NAVIO_UBLOX_PPS 24 | #define RPI_GPIO_23 23 // Pin 16 NAVIO_MPU9250_DRDY 25 | #define RPI_GPIO_24 24 // Pin 18 NAVIO_SPI_PORT_6 26 | #define RPI_GPIO_25 25 // Pin 22 NAVIO_SPI_PORT_5 27 | #define RPI_GPIO_27 27 // Pin 13 NAVIO_PCA9685_OE 28 | #define RPI_GPIO_28 28 // Pin 3 29 | #define RPI_GPIO_29 29 // Pin 4 30 | #define RPI_GPIO_30 30 // Pin 5 31 | #define RPI_GPIO_31 31 // Pin 6 32 | 33 | class Pin { 34 | public: 35 | typedef enum { 36 | GpioModeInput, 37 | GpioModeOutput 38 | } GpioMode; 39 | 40 | Pin(uint8_t pin); 41 | ~Pin(); 42 | 43 | bool init(); 44 | void setMode(GpioMode mode); 45 | uint8_t read() const; 46 | void write(uint8_t value); 47 | void toggle(); 48 | 49 | private: 50 | int getRaspberryPiVersion() const; 51 | Pin (const Pin&); 52 | Pin& operator=(const Pin&); 53 | 54 | uint8_t _pin; 55 | volatile uint32_t *_gpio; 56 | GpioMode _mode; 57 | 58 | bool _deinit(); 59 | }; 60 | 61 | } 62 | 63 | #endif // __NAVIO_GPIO_H__ 64 | -------------------------------------------------------------------------------- /birdeye/birdeye.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | 5 | int main(int argc, char* argv[]) { 6 | char srcfile[200]; 7 | strcpy(srcfile, __FILE__); 8 | std::string dir(srcfile, strrchr(srcfile,'/')+1); 9 | 10 | cv::Mat image = cv::imread(dir + "pic3.jpg"); 11 | 12 | cv::Size board(9, 6), dst(320, 240); 13 | double scale = double(image.size().width) / dst.width; 14 | 15 | std::vector corners; 16 | bool found = cv::findChessboardCorners(image, board, corners); 17 | 18 | cv::Mat gray; 19 | cv::cvtColor(image, gray, cv::COLOR_BGR2GRAY); 20 | cv::cornerSubPix(gray, corners, cv::Size(11,11), cv::Size(-1,-1), 21 | cv::TermCriteria(cv::TermCriteria::EPS|cv::TermCriteria::COUNT, 30, 0.1)); 22 | 23 | cv::Point2f img[4]; 24 | img[0] = corners[0]; 25 | img[1] = corners[board.width - 1]; 26 | img[2] = corners[board.height*board.width - board.width]; 27 | img[3] = corners[board.height*board.width - 1]; 28 | 29 | cv::circle(image, img[0], 9, cv::Scalar(255, 0, 0), 3); 30 | cv::circle(image, img[1], 9, cv::Scalar(0, 255, 0), 3); 31 | cv::circle(image, img[2], 9, cv::Scalar(0, 0, 255), 3); 32 | cv::circle(image, img[3], 9, cv::Scalar(0,255,255), 3); 33 | cv::drawChessboardCorners(image, board, corners, found); 34 | cv::imwrite(dir + "pic-mark.jpg", image); 35 | 36 | for (int i = 0; i < 4; ++i) { 37 | img[i].x /= scale; 38 | img[i].y /= scale; 39 | } 40 | cv::Point2f obj[4]; 41 | obj[0] = img[0]; 42 | obj[1].x = img[1].x; obj[1].y = img[0].y; 43 | obj[2].x = img[0].x; obj[2].y = img[0].y + (img[1].x - img[0].x) * (board.height-1) / (board.width-1); 44 | obj[3].x = img[1].x; obj[3].y = obj[2].y; 45 | 46 | cv::Mat H = cv::getPerspectiveTransform(img, obj); 47 | std::cout << H << std::endl; 48 | 49 | cv::resize(image, image, dst); 50 | cv::warpPerspective(image, image, H, image.size()); 51 | cv::imwrite(dir + "pic-top.jpg", image); 52 | 53 | return 0; 54 | } 55 | -------------------------------------------------------------------------------- /think/queue.h: -------------------------------------------------------------------------------- 1 | #ifndef queue_h 2 | #define queue_h 3 | 4 | #include 5 | 6 | 7 | template 8 | class Queue { 9 | public: 10 | int size; 11 | bool quitting; 12 | 13 | Queue(): size(0), start(0), end(0), quitting(false) { 14 | pthread_mutex_init(&mutex, NULL); 15 | pthread_cond_init(&condNotFull, NULL); 16 | pthread_cond_init(&condNotEmpty, NULL); 17 | } 18 | 19 | ~Queue() { 20 | pthread_mutex_destroy(&mutex); 21 | pthread_cond_destroy(&condNotFull); 22 | pthread_cond_destroy(&condNotEmpty); 23 | } 24 | 25 | void Enqueue(const Value& value) { 26 | pthread_mutex_lock(&mutex); 27 | while (size >= capacity && ! quitting) { 28 | pthread_cond_wait(&condNotFull, &mutex); 29 | } 30 | if (size < capacity) { 31 | buffer[start] = value; 32 | ++size; 33 | ++start; 34 | if (start >= capacity) { 35 | start = 0; 36 | } 37 | } 38 | pthread_mutex_unlock(&mutex); 39 | pthread_cond_broadcast(&condNotEmpty); 40 | } 41 | 42 | Value Dequeue() { 43 | pthread_mutex_lock(&mutex); 44 | while (size <= 0 && ! quitting) { 45 | pthread_cond_wait(&condNotEmpty, &mutex); 46 | } 47 | Value value = buffer[end]; 48 | if (size > 0) { 49 | buffer[end] = Value(); 50 | --size; 51 | ++end; 52 | if (end >= capacity) { 53 | end = 0; 54 | } 55 | } 56 | pthread_mutex_unlock(&mutex); 57 | pthread_cond_broadcast(&condNotFull); 58 | return value; 59 | } 60 | 61 | void Quit() { 62 | if (! quitting) { 63 | quitting = true; 64 | pthread_cond_broadcast(&condNotEmpty); 65 | pthread_cond_broadcast(&condNotFull); 66 | } 67 | } 68 | 69 | int Capacity() const {return capacity;} 70 | 71 | private: 72 | int start; 73 | int end; 74 | pthread_mutex_t mutex; 75 | pthread_cond_t condNotFull; 76 | pthread_cond_t condNotEmpty; 77 | Value buffer[capacity]; 78 | }; 79 | 80 | #endif /* queue_h */ 81 | -------------------------------------------------------------------------------- /drive/Navio/MB85RC04.h: -------------------------------------------------------------------------------- 1 | /* 2 | MB85RC04 driver code is placed under the BSD license. 3 | Written by Egor Fedorov (egor.fedorov@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef MB85RC04_HPP 31 | #define MB85RC04_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include "I2Cdev.h" 38 | 39 | class MB85RC04 40 | { 41 | uint8_t device_address; 42 | 43 | public: 44 | MB85RC04(uint8_t address = 0b1010000); 45 | uint8_t readByte(uint16_t register_address, uint8_t* data); 46 | uint8_t writeByte(uint16_t register_address, uint8_t data); 47 | uint8_t readBytes(uint16_t register_address, uint8_t length, uint8_t* data); 48 | uint8_t writeBytes(uint16_t register_address, uint8_t length, uint8_t* data); 49 | }; 50 | 51 | #endif // MB85RC04_HPP 52 | -------------------------------------------------------------------------------- /drive/Navio/MB85RC256.h: -------------------------------------------------------------------------------- 1 | /* 2 | MB85RC256 driver code is placed under the BSD license. 3 | Written by Egor Fedorov (egor.fedorov@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef MB85RC256_HPP 31 | #define MB85RC256_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include "I2Cdev.h" 39 | 40 | class MB85RC256 41 | { 42 | uint8_t device_address; 43 | 44 | public: 45 | MB85RC256(uint8_t address = 0b1010000); 46 | uint8_t readByte(uint16_t register_address, uint8_t* data); 47 | uint8_t writeByte(uint16_t register_address, uint8_t data); 48 | uint8_t readBytes(uint16_t register_address, uint8_t length, uint8_t* data); 49 | uint8_t writeBytes(uint16_t register_address, uint8_t length, uint8_t* data); 50 | }; 51 | 52 | #endif // MB85RC256_HPP 53 | -------------------------------------------------------------------------------- /drive/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Capture.h" 2 | #include "Compute.h" 3 | #include "ImageLogger.h" 4 | #include "Timer.h" 5 | 6 | #include "Motor.h" 7 | #include "Radio.h" 8 | #include "Navio/Util.h" 9 | 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | static volatile bool quit = false; 18 | 19 | static void Quit(int) { 20 | quit = true; 21 | } 22 | 23 | static const int refreshRate = 40; 24 | 25 | int main(int /*argc*/, char** /*argv*/) { 26 | if (! check_apm()) { 27 | system("sudo modprobe bcm2835-v4l2"); 28 | 29 | // The program runs in four threads. This way, the computer always has something to 30 | // do while waiting for the next captured frame. 31 | // . The Capture thread captures images from the camera. 32 | // . The ImageLogger thread writes every 5th images on disk for later replay. 33 | // . The Compute thread looks for lines in the image, and computes the steering angle. 34 | // . Finally, here the main thread checks the latest steering angle periodically, 35 | // and adjusts the motor speeds accordingly. 36 | 37 | Capture cap(0); 38 | ImageLogger log; 39 | Compute compute(&cap, &log, false); 40 | cap.Start(); 41 | log.Start(); 42 | compute.Start(); 43 | 44 | Motor motor; 45 | Radio rc; 46 | Timer timer; 47 | 48 | signal(SIGHUP, Quit); 49 | signal(SIGINT, Quit); 50 | 51 | timer.Start(); 52 | while (! quit) { 53 | OutputData data; 54 | compute.SwapOutputData(data); 55 | 56 | float throttle = rc.ReadThrottle(); 57 | float leftMotor = throttle; 58 | float rightMotor = throttle; 59 | float steer = data.angle * (20. - 5.*throttle) + 2.*rc.ReadSteer(); 60 | if (steer < 0.) { 61 | rightMotor *= 1. + std::max(-1.F, steer); 62 | } else { 63 | leftMotor *= 1. - std::min( 1.F, steer); 64 | } 65 | motor.SetLeftMotor (leftMotor); 66 | motor.SetRightMotor(rightMotor); 67 | 68 | struct timespec sleep = {0, 1000 * timer.NextSleep(refreshRate, INT_MAX)}; 69 | nanosleep(&sleep, NULL); 70 | } 71 | 72 | motor.SetLeftMotor (0.); 73 | motor.SetRightMotor(0.); 74 | 75 | cap.Stop(); 76 | compute.Stop(); 77 | log.Stop(); 78 | } 79 | return 0; 80 | } 81 | -------------------------------------------------------------------------------- /look/ImageView.cpp: -------------------------------------------------------------------------------- 1 | #include "ImageView.h" 2 | #include "Compute.h" 3 | 4 | 5 | ImageView::ImageView(int imageWidth, int imageHeight) { 6 | glGenTextures(1, &tex); 7 | glBindTexture(GL_TEXTURE_2D, tex); 8 | 9 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE); 10 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE); 11 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR); 12 | glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR); 13 | 14 | glTexImage2D(GL_TEXTURE_2D, 0, GL_RGB, imageWidth, imageHeight, 0, GL_BGR, GL_UNSIGNED_BYTE, NULL); 15 | 16 | glBindTexture(GL_TEXTURE_2D, 0); 17 | } 18 | 19 | ImageView::~ImageView() { 20 | glDeleteTextures(1, &tex); 21 | } 22 | 23 | void ImageView::Draw(const OutputData& data, int viewWidth, int viewHeight, const Mouse& mouse) const { 24 | float w = data.imageAfter.cols, h = data.imageAfter.rows; 25 | 26 | glEnable(GL_TEXTURE_2D); 27 | glDisable(GL_DEPTH_TEST); 28 | glDisable(GL_LIGHTING); 29 | 30 | glViewport(0, 0, viewWidth, viewHeight); 31 | 32 | glMatrixMode(GL_PROJECTION); 33 | glLoadIdentity(); 34 | glOrtho(0., w, h, 0., -1.0, 1.0); 35 | 36 | glMatrixMode(GL_MODELVIEW); 37 | glLoadIdentity(); 38 | 39 | glBindTexture(GL_TEXTURE_2D, tex); 40 | glTexEnvf(GL_TEXTURE_ENV, GL_TEXTURE_ENV_MODE, GL_REPLACE); 41 | 42 | glTexSubImage2D(GL_TEXTURE_2D, 0, 0, 0, w, h, GL_BGR, GL_UNSIGNED_BYTE, data.imageAfter.data); 43 | 44 | glBegin(GL_QUAD_STRIP); 45 | glTexCoord2f(0., 1.); glVertex2f(0, h); 46 | glTexCoord2f(0., 0.); glVertex2f(0, 0); 47 | glTexCoord2f(1., 1.); glVertex2f(w, h); 48 | glTexCoord2f(1., 0.); glVertex2f(w, 0); 49 | glEnd(); 50 | 51 | glBindTexture(GL_TEXTURE_2D, 0); 52 | 53 | glLineWidth(1.); 54 | glColor3f(1., 1., 0.); 55 | glBegin(GL_LINES); 56 | for (Lines::const_iterator l = data.lines.begin(); l != data.lines.end(); ++l) { 57 | const cv::Vec4i& line = *l; 58 | glVertex2i(line[0], line[1]); 59 | glVertex2i(line[2], line[3]); 60 | } 61 | glEnd(); 62 | 63 | if (mouse.down) { 64 | glColor3f(1., 1., 1.); 65 | glBegin(GL_LINE_LOOP); 66 | glVertex2i(mouse.x0, mouse.y0); 67 | glVertex2i(mouse.x1, mouse.y0); 68 | glVertex2i(mouse.x1, mouse.y1); 69 | glVertex2i(mouse.x0, mouse.y1); 70 | glEnd(); 71 | } 72 | 73 | glLineWidth(2.); 74 | if (data.countFramesWithoutLines > 3) { 75 | glColor3f(1., 0., 0.); 76 | } else { 77 | glColor3f(0., 1., 0.); 78 | } 79 | glBegin(GL_LINES); 80 | glVertex2f(w/2, 0.); 81 | glVertex2f(w/2 + h * tan(data.angle), h); 82 | glEnd(); 83 | } 84 | -------------------------------------------------------------------------------- /drive/Navio/MB85RC04.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MB85RC04 driver code is placed under the BSD license. 3 | Written by Egor Fedorov (egor.fedorov@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "MB85RC04.h" 31 | 32 | MB85RC04::MB85RC04(uint8_t address) 33 | { 34 | this->device_address = address; 35 | } 36 | 37 | uint8_t MB85RC04::readByte(uint16_t register_address, uint8_t* data) 38 | { 39 | bool ninth_bit = register_address & 0x100; 40 | uint8_t dev_address = device_address | ninth_bit; 41 | return I2Cdev::readByte(dev_address, register_address, data); 42 | } 43 | 44 | uint8_t MB85RC04::writeByte(uint16_t register_address, uint8_t data) 45 | { 46 | bool ninth_bit = register_address & 0x100; 47 | uint8_t dev_address = device_address | ninth_bit; 48 | return I2Cdev::writeByte(dev_address, register_address, data); 49 | } 50 | 51 | uint8_t MB85RC04::writeBytes(uint16_t register_address, uint8_t length, uint8_t* data) 52 | { 53 | bool ninth_bit = register_address & 0x100; 54 | uint8_t dev_address = device_address | ninth_bit; 55 | 56 | return I2Cdev::writeBytes(dev_address, register_address, 3, data); 57 | } 58 | 59 | uint8_t MB85RC04::readBytes(uint16_t register_address, uint8_t length, uint8_t* data) 60 | { 61 | bool ninth_bit = register_address & 0x100; 62 | uint8_t dev_address = device_address | ninth_bit; 63 | 64 | return I2Cdev::readBytes(dev_address, register_address, 3, data); 65 | } 66 | -------------------------------------------------------------------------------- /drive/Navio/MB85RC256.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MB85RC256 driver code is placed under the BSD license. 3 | Written by Egor Fedorov (egor.fedorov@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "MB85RC256.h" 31 | 32 | MB85RC256::MB85RC256(uint8_t address) 33 | { 34 | this->device_address = address; 35 | } 36 | 37 | uint8_t MB85RC256::readByte(uint16_t register_address, uint8_t* data) 38 | { 39 | return MB85RC256::readBytes(register_address, 1, data); 40 | } 41 | 42 | uint8_t MB85RC256::writeByte(uint16_t register_address, uint8_t data) 43 | { 44 | return MB85RC256::writeBytes(register_address, 1, &data); 45 | } 46 | 47 | uint8_t MB85RC256::writeBytes(uint16_t register_address, uint8_t length, uint8_t* data) 48 | { 49 | uint8_t reg_address_low = register_address; // lower part of the address 50 | uint8_t reg_address_high = register_address >> 8; // higher part of the address 51 | uint8_t msg[length + 1]; 52 | 53 | msg[0] = reg_address_low; 54 | 55 | for (int i = 0; i < length; i++) { 56 | msg[i + 1] = data[i]; 57 | } 58 | 59 | return I2Cdev::writeBytes(this->device_address, reg_address_high, length + 1, msg); 60 | } 61 | 62 | uint8_t MB85RC256::readBytes(uint16_t register_address, uint8_t length, uint8_t* data) 63 | { 64 | uint8_t reg_address_low = register_address; // lower part of the address 65 | uint8_t reg_address_high = register_address >> 8; // higher part of the address 66 | 67 | I2Cdev::writeByte(this->device_address, reg_address_high, reg_address_low); // set the read pointer to the desired address 68 | 69 | return I2Cdev::readBytesNoRegAddress(this->device_address, length, data); 70 | } 71 | 72 | -------------------------------------------------------------------------------- /think/Capture.cpp: -------------------------------------------------------------------------------- 1 | #include "Capture.h" 2 | #include "Timer.h" 3 | #include 4 | #include 5 | 6 | 7 | static const int cameraWidth = 320, cameraHeight = 240; 8 | static const float cameraFPS = -1.; 9 | static const int videoFileFPS = 15; 10 | 11 | 12 | Capture::Capture(int device) 13 | : cap(device) 14 | , fps(-1) 15 | , canDropFrame(false) 16 | { 17 | printTimeStats = true; 18 | playing = true; 19 | 20 | #if 1 21 | imageWidth = cameraWidth; 22 | imageHeight = cameraHeight; 23 | cap.set(cv::CAP_PROP_FRAME_WIDTH, cameraWidth); 24 | cap.set(cv::CAP_PROP_FRAME_HEIGHT, cameraHeight); 25 | #else 26 | imageWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH); 27 | imageHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT); 28 | std::cerr << "camera: " << imageWidth << " x " << imageHeight << std::endl; 29 | #endif 30 | 31 | if (cameraFPS > 0) { 32 | cap.set(cv::CAP_PROP_FPS, cameraFPS); 33 | } 34 | } 35 | 36 | Capture::Capture(const cv::String& filename, int api) 37 | : cap(filename, api) 38 | , fps(videoFileFPS) 39 | , canDropFrame(false) 40 | { 41 | printTimeStats = false; 42 | playing = true; 43 | imageWidth = cap.get(cv::CAP_PROP_FRAME_WIDTH); 44 | imageHeight = cap.get(cv::CAP_PROP_FRAME_HEIGHT); 45 | } 46 | 47 | void Capture::BackgroundLoop() { 48 | cv::Mat image; 49 | int frameCount = 0; 50 | Timer timer; 51 | 52 | timer.Start(); 53 | while (! commands.quitting) { 54 | 55 | bool playThisFrame = true; 56 | switch (NextCommand(frameCount, timer)) { 57 | case Rewind: 58 | cap.set(cv::CAP_PROP_POS_FRAMES, 0.); 59 | break; 60 | case NextFrame: 61 | break; 62 | case PrevFrame: { 63 | double prev = cap.get(cv::CAP_PROP_POS_FRAMES) - 2.; 64 | if (prev >= 0.) { 65 | cap.set(cv::CAP_PROP_POS_FRAMES, prev); 66 | } 67 | } break; 68 | default: 69 | playThisFrame = playing; 70 | break; 71 | } 72 | 73 | if (playThisFrame) { 74 | if (cap.read(image)) { 75 | if (canDropFrame && imagesCaptured.size >= imagesCaptured.Capacity()) { 76 | std::cerr << "Capture dropped frame." << std::endl; 77 | } else { 78 | imagesCaptured.Enqueue(image); 79 | 80 | ++frameCount; 81 | struct timespec sleep = {0, fps > 0 ? 1000 * timer.NextSleep(fps, frameCount) : 0}; 82 | if (sleep.tv_nsec > 0) { 83 | nanosleep(&sleep, NULL); 84 | } 85 | } 86 | } else if (playing) { 87 | playing = false; 88 | PrintTimeStats(frameCount, timer); 89 | } 90 | } 91 | } 92 | if (playing) { 93 | PrintTimeStats(frameCount, timer); 94 | } 95 | } 96 | 97 | static void* CaptureThread(void* cap) { 98 | ((Capture*)cap)->BackgroundLoop(); 99 | return 0; 100 | } 101 | 102 | void Capture::Start() { 103 | pthread_create(&thread, NULL, CaptureThread, this); 104 | } 105 | 106 | void Capture::Stop() { 107 | commands.Quit(); 108 | pthread_join(thread, NULL); 109 | } 110 | -------------------------------------------------------------------------------- /drive/Navio/SPIdev.h: -------------------------------------------------------------------------------- 1 | /* 2 | SPIDev driver code is placed under the BSD license. 3 | Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef _SPIDEV_H_ 31 | #define _SPIDEV_H_ 32 | 33 | //#define _XOPEN_SOURCE 600 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | class SPIdev { 46 | public: 47 | SPIdev() 48 | { 49 | } 50 | 51 | static int transfer(const char *spidev, 52 | unsigned char *tx, 53 | unsigned char *rx, 54 | unsigned int length, 55 | unsigned int speed_hz = 1000000, 56 | unsigned char bits_per_word = 8, 57 | unsigned short delay_usecs = 0) 58 | { 59 | spi_ioc_transfer spi_transfer; 60 | 61 | memset(&spi_transfer, 0, sizeof(spi_ioc_transfer)); 62 | 63 | spi_transfer.tx_buf = (unsigned long)tx; 64 | spi_transfer.rx_buf = (unsigned long)rx; 65 | spi_transfer.len = length; 66 | spi_transfer.speed_hz = speed_hz; 67 | spi_transfer.bits_per_word = bits_per_word; 68 | spi_transfer.delay_usecs = delay_usecs; 69 | 70 | int spi_fd = ::open(spidev, O_RDWR); 71 | 72 | if (spi_fd < 0 ) { 73 | printf("Error: Can not open SPI device\n"); 74 | 75 | return -1; 76 | } 77 | 78 | int status = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer); 79 | 80 | ::close(spi_fd); 81 | 82 | // Debug information 83 | /* 84 | printf("Tx: "); 85 | for (int i = 0; i < length; i++) 86 | printf("%x ", tx[i]); 87 | printf("\n"); 88 | 89 | printf("Rx: "); 90 | for (int i = 0; i < length; i++) 91 | printf("%x ", rx[i]); 92 | printf("\n"); 93 | */ 94 | 95 | return status; 96 | } 97 | }; 98 | 99 | #endif //_SPIDEV_H_ 100 | -------------------------------------------------------------------------------- /drive/Navio/MS5611.h: -------------------------------------------------------------------------------- 1 | /* 2 | MS5611 driver code is placed under the BSD license. 3 | Copyright (c) 2014, Emlid Limited, www.emlid.com 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the Emlid Limited nor the names of its contributors 14 | may be used to endorse or promote products derived from this software 15 | without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #ifndef MS5611_HPP 30 | #define MS5611_HPP 31 | 32 | #include "I2Cdev.h" 33 | #include 34 | #include 35 | #include 36 | 37 | #define MS5611_ADDRESS_CSB_LOW 0x76 38 | #define MS5611_ADDRESS_CSB_HIGH 0x77 39 | #define MS5611_DEFAULT_ADDRESS MS5611_ADDRESS_CSB_HIGH 40 | 41 | #define MS5611_RA_ADC 0x00 42 | #define MS5611_RA_RESET 0x1E 43 | 44 | #define MS5611_RA_C0 0xA0 45 | #define MS5611_RA_C1 0xA2 46 | #define MS5611_RA_C2 0xA4 47 | #define MS5611_RA_C3 0xA6 48 | #define MS5611_RA_C4 0xA8 49 | #define MS5611_RA_C5 0xAA 50 | #define MS5611_RA_C6 0xAC 51 | #define MS5611_RA_C7 0xAE 52 | 53 | #define MS5611_RA_D1_OSR_256 0x40 54 | #define MS5611_RA_D1_OSR_512 0x42 55 | #define MS5611_RA_D1_OSR_1024 0x44 56 | #define MS5611_RA_D1_OSR_2048 0x46 57 | #define MS5611_RA_D1_OSR_4096 0x48 58 | 59 | #define MS5611_RA_D2_OSR_256 0x50 60 | #define MS5611_RA_D2_OSR_512 0x52 61 | #define MS5611_RA_D2_OSR_1024 0x54 62 | #define MS5611_RA_D2_OSR_2048 0x56 63 | #define MS5611_RA_D2_OSR_4096 0x58 64 | 65 | class MS5611 { 66 | public: 67 | MS5611(uint8_t address = MS5611_DEFAULT_ADDRESS); 68 | 69 | void initialize(); 70 | bool testConnection(); 71 | 72 | void refreshPressure(uint8_t OSR = MS5611_RA_D1_OSR_4096); 73 | void readPressure(); 74 | 75 | void refreshTemperature(uint8_t OSR = MS5611_RA_D2_OSR_4096); 76 | void readTemperature(); 77 | 78 | void calculatePressureAndTemperature(); 79 | void update(); 80 | 81 | float getTemperature(); 82 | float getPressure(); 83 | 84 | private: 85 | uint8_t devAddr; // I2C device adress 86 | uint16_t C1, C2, C3, C4, C5, C6; // Calibration data 87 | uint32_t D1, D2; // Raw measurement data 88 | float TEMP; // Calculated temperature 89 | float PRES; // Calculated pressure 90 | }; 91 | 92 | #endif // MS5611_HPP 93 | -------------------------------------------------------------------------------- /drive/Navio/PCA9685.h: -------------------------------------------------------------------------------- 1 | /* 2 | PCA9685 driver code is placed under the BSD license. 3 | Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef PCA9685_HPP 31 | #define PCA9685_HPP 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include "I2Cdev.h" 41 | 42 | #define PCA9685_DEFAULT_ADDRESS 0x40 // All address pins low, Navio default 43 | 44 | #define PCA9685_RA_MODE1 0x00 45 | #define PCA9685_RA_MODE2 0x01 46 | #define PCA9685_RA_LED0_ON_L 0x06 47 | #define PCA9685_RA_LED0_ON_H 0x07 48 | #define PCA9685_RA_LED0_OFF_L 0x08 49 | #define PCA9685_RA_LED0_OFF_H 0x09 50 | #define PCA9685_RA_ALL_LED_ON_L 0xFA 51 | #define PCA9685_RA_ALL_LED_ON_H 0xFB 52 | #define PCA9685_RA_ALL_LED_OFF_L 0xFC 53 | #define PCA9685_RA_ALL_LED_OFF_H 0xFD 54 | #define PCA9685_RA_PRE_SCALE 0xFE 55 | 56 | #define PCA9685_MODE1_RESTART_BIT 7 57 | #define PCA9685_MODE1_EXTCLK_BIT 6 58 | #define PCA9685_MODE1_AI_BIT 5 59 | #define PCA9685_MODE1_SLEEP_BIT 4 60 | #define PCA9685_MODE1_SUB1_BIT 3 61 | #define PCA9685_MODE1_SUB2_BIT 2 62 | #define PCA9685_MODE1_SUB3_BIT 1 63 | #define PCA9685_MODE1_ALLCALL_BIT 0 64 | 65 | #define PCA9685_MODE2_INVRT_BIT 4 66 | #define PCA9685_MODE2_OCH_BIT 3 67 | #define PCA9685_MODE2_OUTDRV_BIT 2 68 | #define PCA9685_MODE2_OUTNE1_BIT 1 69 | #define PCA9685_MODE2_OUTNE0_BIT 0 70 | 71 | class PCA9685 { 72 | public: 73 | PCA9685(uint8_t address = PCA9685_DEFAULT_ADDRESS); 74 | 75 | void initialize(); 76 | bool testConnection(); 77 | 78 | float getFrequency(); 79 | void setFrequency(float frequency); 80 | 81 | void sleep(); 82 | void restart(); 83 | 84 | void setPWM(uint8_t channel, uint16_t offset, uint16_t length); 85 | void setPWM(uint8_t channel, uint16_t length); 86 | void setPWMmS(uint8_t channel, float length_mS); 87 | void setPWMuS(uint8_t channel, float length_uS); 88 | 89 | void setAllPWM(uint16_t offset, uint16_t length); 90 | void setAllPWM(uint16_t length); 91 | void setAllPWMmS(float length_mS); 92 | void setAllPWMuS(float length_uS); 93 | 94 | private: 95 | uint8_t devAddr; 96 | float frequency; 97 | }; 98 | 99 | #endif // PCA9685_ HPP 100 | -------------------------------------------------------------------------------- /drive/Navio/Ublox.h: -------------------------------------------------------------------------------- 1 | /* 2 | GPS driver dcode is placed under the BSD license. 3 | Written by Egor Fedorov (egor.fedorov@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | //#define _XOPEN_SOURCE 600 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include "SPIdev.h" 45 | 46 | static const int buffer_length = 1024; 47 | 48 | class UBXScanner { 49 | public: 50 | enum State 51 | { 52 | Sync1, 53 | Sync2, 54 | Class, 55 | ID, 56 | Length1, 57 | Length2, 58 | Payload, 59 | CK_A, 60 | CK_B, 61 | Done 62 | }; 63 | 64 | unsigned char message[buffer_length]; // Buffer for UBX message 65 | unsigned int message_length; // Length of the received message 66 | 67 | private: 68 | unsigned int position; // Indicates current buffer offset 69 | unsigned int payload_length; // Length of current message payload 70 | State state; // Current scanner state 71 | 72 | public: 73 | UBXScanner(); 74 | unsigned char* getMessage(); 75 | unsigned int getMessageLength(); 76 | unsigned int getPosition(); 77 | void reset(); 78 | int update(unsigned char data); 79 | }; 80 | 81 | class UBXParser{ 82 | private: 83 | UBXScanner* scanner; // pointer to the scanner, which finds the messages in the data stream 84 | unsigned char* message; // pointer to the scanner's message buffer 85 | unsigned int length; // current message length 86 | unsigned int position; // current message end position 87 | 88 | public: 89 | UBXParser(UBXScanner* ubxsc); 90 | void updateMessageData(); 91 | int decodeMessage(std::vector& data); 92 | int checkMessage(); 93 | }; 94 | 95 | class Ublox { 96 | public: 97 | enum message_t 98 | { 99 | NAV_POSLLH = 0x0102, 100 | NAV_STATUS = 0x0103 101 | }; 102 | 103 | private: 104 | std::string spi_device_name; 105 | UBXScanner* scanner; 106 | UBXParser* parser; 107 | 108 | public: 109 | Ublox(std::string name = "/dev/spidev0.0"); 110 | Ublox(std::string name, UBXScanner* scan, UBXParser* pars); 111 | int enableNAV_POSLLH(); 112 | int enableNAV_STATUS(); 113 | int testConnection(); 114 | int decodeMessages(); 115 | int decodeSingleMessage(message_t msg, std::vector& position_data); 116 | }; // end of ublox class def 117 | -------------------------------------------------------------------------------- /drive/Navio/gpio.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | 13 | #include "gpio.h" 14 | 15 | #define LOW 0 16 | #define HIGH 1 17 | 18 | /* Raspberry Pi GPIO memory */ 19 | #define BCM2708_PERI_BASE 0x20000000 20 | #define BCM2709_PERI_BASE 0x3F000000 21 | #define BCM2835_PERI_BASE 0x20000000 22 | #define GPIO_BASE(address) (address + 0x200000) 23 | #define PAGE_SIZE (4*1024) 24 | #define BLOCK_SIZE (4*1024) 25 | 26 | /* GPIO setup. Always use INP_GPIO(x) before OUT_GPIO(x) or SET_GPIO_ALT(x,y) */ 27 | #define GPIO_MODE_IN(g) *(_gpio+((g)/10)) &= ~(7<<(((g)%10)*3)) 28 | #define GPIO_MODE_OUT(g) *(_gpio+((g)/10)) |= (1<<(((g)%10)*3)) 29 | #define GPIO_MODE_ALT(g,a) *(_gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3)) 30 | #define GPIO_SET_HIGH *(_gpio+7) // sets bits which are 1 31 | #define GPIO_SET_LOW *(_gpio+10) // clears bits which are 1 32 | #define GPIO_GET(g) (*(_gpio+13)&(1<(_gpio), BLOCK_SIZE) < 0) { 55 | warnx("unmap failed"); 56 | return false; 57 | } 58 | return true; 59 | } 60 | 61 | bool Pin::init() 62 | { 63 | int mem_fd; 64 | if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) { 65 | warn("/dev/mem cannot be opened"); 66 | return false; 67 | } 68 | 69 | uint32_t address; 70 | int version = getRaspberryPiVersion(); 71 | if (version == 1) { 72 | address = GPIO_BASE(BCM2708_PERI_BASE); 73 | } else if (version == 2) { 74 | address = GPIO_BASE(BCM2709_PERI_BASE); 75 | } else if (version == 3) { 76 | address = GPIO_BASE(BCM2835_PERI_BASE); 77 | } 78 | 79 | void *gpio_map = mmap( 80 | NULL, /* any adddress in our space will do */ 81 | BLOCK_SIZE, /* map length */ 82 | PROT_READ|PROT_WRITE, /* enable reading & writting to mapped memory */ 83 | MAP_SHARED, /* shared with other processes */ 84 | mem_fd, /* file to map */ 85 | address /* offset to GPIO peripheral */ 86 | ); 87 | 88 | if (gpio_map == MAP_FAILED) { 89 | warn("cannot mmap memory"); 90 | return false; 91 | } 92 | 93 | /* No need to keep mem_fd open after mmap */ 94 | if (close(mem_fd) < 0) { 95 | warn("cannot close mem_fd"); 96 | return false; 97 | } 98 | 99 | _gpio = reinterpret_cast(gpio_map); // Always use volatile pointer! 100 | 101 | return true; 102 | } 103 | 104 | void Pin::setMode(GpioMode mode) 105 | { 106 | if (mode == GpioModeInput) { 107 | GPIO_MODE_IN(_pin); 108 | } else { 109 | GPIO_MODE_IN(_pin); 110 | GPIO_MODE_OUT(_pin); 111 | } 112 | 113 | _mode = mode; 114 | } 115 | 116 | uint8_t Pin::read() const 117 | { 118 | uint32_t value = GPIO_GET(_pin); 119 | return value ? 1: 0; 120 | } 121 | 122 | void Pin::write(uint8_t value) 123 | { 124 | if (_mode != GpioModeOutput) { 125 | warnx("no effect because mode is not set"); 126 | } 127 | 128 | if (value == LOW) { 129 | GPIO_SET_LOW = 1 << _pin; 130 | } else { 131 | GPIO_SET_HIGH = 1 << _pin; 132 | } 133 | } 134 | 135 | void Pin::toggle() 136 | { 137 | write(!read()); 138 | } 139 | 140 | int Pin::getRaspberryPiVersion() const 141 | { 142 | char buffer[MAX_SIZE_LINE]; 143 | const char* hardware_description_entry = "Hardware"; 144 | const char* v1 = "BCM2708"; 145 | const char* v2 = "BCM2709"; 146 | const char* v3 = "BCM2835"; 147 | char* flag; 148 | FILE* fd; 149 | 150 | fd = fopen("/proc/cpuinfo", "r"); 151 | 152 | while (fgets(buffer, MAX_SIZE_LINE, fd) != NULL) { 153 | flag = strstr(buffer, hardware_description_entry); 154 | 155 | if (flag != NULL) { 156 | if (strstr(buffer, v3) != NULL) { 157 | fclose(fd); 158 | return 3; 159 | } else if (strstr(buffer, v2) != NULL) { 160 | fclose(fd); 161 | return 2; 162 | } else if (strstr(buffer, v1) != NULL) { 163 | fclose(fd); 164 | return 1; 165 | } 166 | } 167 | } 168 | 169 | /* defaults to 1 */ 170 | fprintf(stderr, "Could not detect RPi version, defaulting to 1\n"); 171 | fclose(fd); 172 | return 1; 173 | } 174 | -------------------------------------------------------------------------------- /drive/Navio/I2Cdev.h: -------------------------------------------------------------------------------- 1 | // i2cDev library collection - Main I2C device class header file 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 7 | // - add compiler warnings when using outdated or IDE or limited i2cDev implementation 8 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 9 | // 2011-10-03 - added automatic Arduino version detection for ease of use 10 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 11 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 12 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 13 | // 2011-08-02 - added support for 16-bit registers 14 | // - fixed incorrect Doxygen comments on some methods 15 | // - added timeout value for read operations (thanks mem @ Arduino forums) 16 | // 2011-07-30 - changed read/write function structures to return success or byte counts 17 | // - made all methods static for multi-device memory savings 18 | // 2011-07-28 - initial release 19 | 20 | /* ============================================ 21 | I2Cdev device library code is placed under the MIT license 22 | Copyright (c) 2012 Jeff Rowberg 23 | 24 | Permission is hereby granted, free of charge, to any person obtaining a copy 25 | of this software and associated documentation files (the "Software"), to deal 26 | in the Software without restriction, including without limitation the rights 27 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28 | copies of the Software, and to permit persons to whom the Software is 29 | furnished to do so, subject to the following conditions: 30 | 31 | The above copyright notice and this permission notice shall be included in 32 | all copies or substantial portions of the Software. 33 | 34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40 | THE SOFTWARE. 41 | =============================================== 42 | */ 43 | 44 | #ifndef _I2Cdev_H_ 45 | #define _I2Cdev_H_ 46 | 47 | #define RASPBERRY_PI_I2C "/dev/i2c-1" 48 | #define BANANA_PI_I2C "/dev/i2c-2" 49 | 50 | #define I2CDEV RASPBERRY_PI_I2C 51 | 52 | #ifndef TRUE 53 | #define TRUE (1==1) 54 | #define FALSE (0==1) 55 | #endif 56 | 57 | #include 58 | 59 | class I2Cdev { 60 | public: 61 | I2Cdev(); 62 | 63 | static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 64 | static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 65 | static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 66 | static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 67 | static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 68 | static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 69 | static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 70 | static int8_t readBytesNoRegAddress(uint8_t devAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout); 71 | static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout); 72 | 73 | static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data); 74 | static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data); 75 | static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data); 76 | static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data); 77 | static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data); 78 | static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data); 79 | static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data); 80 | static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data); 81 | 82 | static uint16_t readTimeout; 83 | }; 84 | 85 | #endif /* _I2Cdev_H_ */ 86 | -------------------------------------------------------------------------------- /drive/Navio/MS5611.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | MS5611 driver code is placed under the BSD license. 3 | Copyright (c) 2014, Emlid Limited, www.emlid.com 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | * Redistributions of source code must retain the above copyright 9 | notice, this list of conditions and the following disclaimer. 10 | * Redistributions in binary form must reproduce the above copyright 11 | notice, this list of conditions and the following disclaimer in the 12 | documentation and/or other materials provided with the distribution. 13 | * Neither the name of the Emlid Limited nor the names of its contributors 14 | may be used to endorse or promote products derived from this software 15 | without specific prior written permission. 16 | 17 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 18 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 19 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 20 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 21 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | */ 28 | 29 | #include "MS5611.h" 30 | 31 | /** MS5611 constructor. 32 | * @param address I2C address 33 | * @see MS5611_DEFAULT_ADDRESS 34 | */ 35 | MS5611::MS5611(uint8_t address) { 36 | this->devAddr = address; 37 | } 38 | 39 | /** Power on and prepare for general usage. 40 | * This method reads coefficients stored in PROM. 41 | */ 42 | void MS5611::initialize() { 43 | // Reading 6 calibration data values 44 | uint8_t buff[2]; 45 | I2Cdev::readBytes(devAddr, MS5611_RA_C1, 2, buff); 46 | C1 = buff[0]<<8 | buff[1]; 47 | I2Cdev::readBytes(devAddr, MS5611_RA_C2, 2, buff); 48 | C2 = buff[0]<<8 | buff[1]; 49 | I2Cdev::readBytes(devAddr, MS5611_RA_C3, 2, buff); 50 | C3 = buff[0]<<8 | buff[1]; 51 | I2Cdev::readBytes(devAddr, MS5611_RA_C4, 2, buff); 52 | C4 = buff[0]<<8 | buff[1]; 53 | I2Cdev::readBytes(devAddr, MS5611_RA_C5, 2, buff); 54 | C5 = buff[0]<<8 | buff[1]; 55 | I2Cdev::readBytes(devAddr, MS5611_RA_C6, 2, buff); 56 | C6 = buff[0]<<8 | buff[1]; 57 | 58 | update(); 59 | } 60 | 61 | /** Verify the I2C connection. 62 | * @return True if connection is valid, false otherwise 63 | */ 64 | bool MS5611::testConnection() { 65 | uint8_t data; 66 | int8_t status = I2Cdev::readByte(devAddr, MS5611_RA_C0, &data); 67 | if (status > 0) 68 | return true; 69 | else 70 | return false; 71 | } 72 | 73 | /** Initiate the process of pressure measurement 74 | * @param OSR value 75 | * @see MS5611_RA_D1_OSR_4096 76 | */ 77 | void MS5611::refreshPressure(uint8_t OSR) { 78 | I2Cdev::writeBytes(devAddr, OSR, 0, 0); 79 | } 80 | 81 | /** Read pressure value 82 | */ 83 | void MS5611::readPressure() { 84 | // 85 | uint8_t buffer[3]; 86 | I2Cdev::readBytes(devAddr, MS5611_RA_ADC, 3, buffer); 87 | D1 = (buffer[0] << 16) | (buffer[1] << 8) | buffer[2]; 88 | } 89 | 90 | /** Initiate the process of temperature measurement 91 | * @param OSR value 92 | * @see MS5611_RA_D2_OSR_4096 93 | */ 94 | void MS5611::refreshTemperature(uint8_t OSR) { 95 | I2Cdev::writeBytes(devAddr, OSR, 0, 0); 96 | } 97 | 98 | /** Read temperature value 99 | */ 100 | void MS5611::readTemperature() { 101 | uint8_t buffer[3]; 102 | I2Cdev::readBytes(devAddr, MS5611_RA_ADC, 3, buffer); 103 | D2 = (buffer[0] << 16) | (buffer[1] << 8) | buffer[2]; 104 | } 105 | 106 | /** Calculate temperature and pressure calculations and perform compensation 107 | * More info about these calculations is available in the datasheet. 108 | */ 109 | void MS5611::calculatePressureAndTemperature() { 110 | float dT = D2 - C5 * pow(2, 8); 111 | TEMP = (2000 + ((dT * C6) / pow(2, 23))); 112 | float OFF = C2 * pow(2, 16) + (C4 * dT) / pow(2, 7); 113 | float SENS = C1 * pow(2, 15) + (C3 * dT) / pow(2, 8); 114 | 115 | float T2, OFF2, SENS2; 116 | 117 | if (TEMP >= 2000) 118 | { 119 | T2 = 0; 120 | OFF2 = 0; 121 | SENS2 = 0; 122 | } 123 | if (TEMP < 2000) 124 | { 125 | T2 = dT * dT / pow(2, 31); 126 | OFF2 = 5 * pow(TEMP - 2000, 2) / 2; 127 | SENS2 = OFF2 / 2; 128 | } 129 | if (TEMP < -1500) 130 | { 131 | OFF2 = OFF2 + 7 * pow(TEMP + 1500, 2); 132 | SENS2 = SENS2 + 11 * pow(TEMP + 1500, 2) / 2; 133 | } 134 | 135 | TEMP = TEMP - T2; 136 | OFF = OFF - OFF2; 137 | SENS = SENS - SENS2; 138 | 139 | // Final calculations 140 | PRES = ((D1 * SENS) / pow(2, 21) - OFF) / pow(2, 15) / 100; 141 | TEMP = TEMP / 100; 142 | } 143 | 144 | /** Perform pressure and temperature reading and calculation at once. 145 | * Contains sleeps, better perform operations separately. 146 | */ 147 | void MS5611::update() { 148 | refreshPressure(); 149 | usleep(10000); // Waiting for pressure data ready 150 | readPressure(); 151 | 152 | refreshTemperature(); 153 | usleep(10000); // Waiting for temperature data ready 154 | readTemperature(); 155 | 156 | calculatePressureAndTemperature(); 157 | } 158 | 159 | /** Get calculated temperature value 160 | @return Temperature in degrees of Celsius 161 | */ 162 | float MS5611::getTemperature() { 163 | return TEMP; 164 | } 165 | 166 | /** Get calculated pressure value 167 | @return Pressure in millibars 168 | */ 169 | float MS5611::getPressure() { 170 | return PRES; 171 | } 172 | -------------------------------------------------------------------------------- /drive/Navio/LSM9DS1.h: -------------------------------------------------------------------------------- 1 | /* 2 | Written by Alexey Bulatov (alexey.bulatov@emlid.com) for Raspberry Pi 3 | */ 4 | 5 | #ifndef _LSM9DS1_H 6 | #define _LSM9DS1_H 7 | 8 | #include 9 | #include "SPIdev.h" 10 | #include "InertialSensor.h" 11 | 12 | class LSM9DS1 : public InertialSensor 13 | { 14 | public: 15 | LSM9DS1(); 16 | 17 | bool initialize(); 18 | bool probe(); 19 | void update(); 20 | 21 | private: 22 | unsigned int WriteReg(const char *dev, uint8_t WriteAddr, uint8_t WriteData); 23 | unsigned int ReadReg(const char *dev, uint8_t ReadAddr); 24 | void ReadRegs(const char *dev, uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes); 25 | 26 | void set_gyro_scale(int scale); 27 | void set_acc_scale(int scale); 28 | void set_mag_scale(int scale); 29 | 30 | void rotate(); 31 | 32 | float gyro_scale; 33 | float acc_scale; 34 | float mag_scale; 35 | }; 36 | 37 | #endif //_LSM9DS1_H 38 | 39 | // who am I values 40 | #define WHO_AM_I_ACC_GYRO 0x68 41 | #define WHO_AM_I_MAG 0x3D 42 | 43 | // Accelerometer and Gyroscope registers 44 | #define LSM9DS1XG_ACT_THS 0x04 45 | #define LSM9DS1XG_ACT_DUR 0x05 46 | #define LSM9DS1XG_INT_GEN_CFG_XL 0x06 47 | #define LSM9DS1XG_INT_GEN_THS_X_XL 0x07 48 | #define LSM9DS1XG_INT_GEN_THS_Y_XL 0x08 49 | #define LSM9DS1XG_INT_GEN_THS_Z_XL 0x09 50 | #define LSM9DS1XG_INT_GEN_DUR_XL 0x0A 51 | #define LSM9DS1XG_REFERENCE_G 0x0B 52 | #define LSM9DS1XG_INT1_CTRL 0x0C 53 | #define LSM9DS1XG_INT2_CTRL 0x0D 54 | #define LSM9DS1XG_WHO_AM_I 0x0F // should return 0x68 55 | #define LSM9DS1XG_CTRL_REG1_G 0x10 56 | #define LSM9DS1XG_CTRL_REG2_G 0x11 57 | #define LSM9DS1XG_CTRL_REG3_G 0x12 58 | #define LSM9DS1XG_ORIENT_CFG_G 0x13 59 | #define LSM9DS1XG_INT_GEN_SRC_G 0x14 60 | #define LSM9DS1XG_OUT_TEMP_L 0x15 61 | #define LSM9DS1XG_OUT_TEMP_H 0x16 62 | #define LSM9DS1XG_STATUS_REG 0x17 63 | #define LSM9DS1XG_OUT_X_L_G 0x18 64 | #define LSM9DS1XG_OUT_X_H_G 0x19 65 | #define LSM9DS1XG_OUT_Y_L_G 0x1A 66 | #define LSM9DS1XG_OUT_Y_H_G 0x1B 67 | #define LSM9DS1XG_OUT_Z_L_G 0x1C 68 | #define LSM9DS1XG_OUT_Z_H_G 0x1D 69 | #define LSM9DS1XG_CTRL_REG4 0x1E 70 | #define LSM9DS1XG_CTRL_REG5_XL 0x1F 71 | #define LSM9DS1XG_CTRL_REG6_XL 0x20 72 | #define LSM9DS1XG_CTRL_REG7_XL 0x21 73 | #define LSM9DS1XG_CTRL_REG8 0x22 74 | #define LSM9DS1XG_CTRL_REG9 0x23 75 | #define LSM9DS1XG_CTRL_REG10 0x24 76 | #define LSM9DS1XG_INT_GEN_SRC_XL 0x26 77 | #define LSM9DS1XG_OUT_X_L_XL 0x28 78 | #define LSM9DS1XG_OUT_X_H_XL 0x29 79 | #define LSM9DS1XG_OUT_Y_L_XL 0x2A 80 | #define LSM9DS1XG_OUT_Y_H_XL 0x2B 81 | #define LSM9DS1XG_OUT_Z_L_XL 0x2C 82 | #define LSM9DS1XG_OUT_Z_H_XL 0x2D 83 | #define LSM9DS1XG_FIFO_CTRL 0x2E 84 | #define LSM9DS1XG_FIFO_SRC 0x2F 85 | #define LSM9DS1XG_INT_GEN_CFG_G 0x30 86 | #define LSM9DS1XG_INT_GEN_THS_XH_G 0x31 87 | #define LSM9DS1XG_INT_GEN_THS_XL_G 0x32 88 | #define LSM9DS1XG_INT_GEN_THS_YH_G 0x33 89 | #define LSM9DS1XG_INT_GEN_THS_YL_G 0x34 90 | #define LSM9DS1XG_INT_GEN_THS_ZH_G 0x35 91 | #define LSM9DS1XG_INT_GEN_THS_ZL_G 0x36 92 | #define LSM9DS1XG_INT_GEN_DUR_G 0x37 93 | 94 | // Magnetometer registers 95 | #define LSM9DS1M_OFFSET_X_REG_L_M 0x05 96 | #define LSM9DS1M_OFFSET_X_REG_H_M 0x06 97 | #define LSM9DS1M_OFFSET_Y_REG_L_M 0x07 98 | #define LSM9DS1M_OFFSET_Y_REG_H_M 0x08 99 | #define LSM9DS1M_OFFSET_Z_REG_L_M 0x09 100 | #define LSM9DS1M_OFFSET_Z_REG_H_M 0x0A 101 | #define LSM9DS1M_WHO_AM_I 0x0F // should return 0x3D 102 | #define LSM9DS1M_CTRL_REG1_M 0x20 103 | #define LSM9DS1M_CTRL_REG2_M 0x21 104 | #define LSM9DS1M_CTRL_REG3_M 0x22 105 | #define LSM9DS1M_CTRL_REG4_M 0x23 106 | #define LSM9DS1M_CTRL_REG5_M 0x24 107 | #define LSM9DS1M_STATUS_REG_M 0x27 108 | #define LSM9DS1M_OUT_X_L_M 0x28 109 | #define LSM9DS1M_OUT_X_H_M 0x29 110 | #define LSM9DS1M_OUT_Y_L_M 0x2A 111 | #define LSM9DS1M_OUT_Y_H_M 0x2B 112 | #define LSM9DS1M_OUT_Z_L_M 0x2C 113 | #define LSM9DS1M_OUT_Z_H_M 0x2D 114 | #define LSM9DS1M_INT_CFG_M 0x30 115 | #define LSM9DS1M_INT_SRC_M 0x31 116 | #define LSM9DS1M_INT_THS_L_M 0x32 117 | #define LSM9DS1M_INT_THS_H_M 0x33 118 | 119 | // Configuration bits Accelerometer and Gyroscope 120 | #define BITS_XEN_G 0x08 121 | #define BITS_YEN_G 0x10 122 | #define BITS_ZEN_G 0x20 123 | #define BITS_XEN_XL 0x08 124 | #define BITS_YEN_XL 0x10 125 | #define BITS_ZEN_XL 0x20 126 | #define BITS_ODR_G_14900mHZ 0x20 127 | #define BITS_ODR_G_59500mHZ 0x40 128 | #define BITS_ODR_G_119HZ 0x60 129 | #define BITS_ODR_G_238HZ 0x80 130 | #define BITS_ODR_G_476HZ 0xA0 131 | #define BITS_ODR_G_952HZ 0xC0 132 | #define BITS_ODR_XL_10HZ 0x20 133 | #define BITS_ODR_XL_50HZ 0x40 134 | #define BITS_ODR_XL_119HZ 0x60 135 | #define BITS_ODR_XL_238HZ 0x80 136 | #define BITS_ODR_XL_476HZ 0xA0 137 | #define BITS_ODR_XL_952HZ 0xC0 138 | #define BITS_FS_G_MASK 0xE3 139 | #define BITS_FS_G_245DPS 0x00 140 | #define BITS_FS_G_500DPS 0x08 141 | #define BITS_FS_G_2000DPS 0x18 142 | #define BITS_FS_XL_MASK 0xE7 143 | #define BITS_FS_XL_2G 0x00 144 | #define BITS_FS_XL_4G 0x10 145 | #define BITS_FS_XL_8G 0x18 146 | #define BITS_FS_XL_16G 0x08 147 | 148 | // Configuration bits Magnetometer 149 | #define BITS_TEMP_COMP 0x80 150 | #define BITS_OM_LOW 0x00 151 | #define BITS_OM_MEDIUM 0x20 152 | #define BITS_OM_HIGH 0x40 153 | #define BITS_OM_ULTRA_HIGH 0x60 154 | #define BITS_ODR_M_625mHZ 0x00 155 | #define BITS_ODR_M_1250mHZ 0x04 156 | #define BITS_ODR_M_250mHZ 0x08 157 | #define BITS_ODR_M_5HZ 0x0C 158 | #define BITS_ODR_M_10HZ 0x10 159 | #define BITS_ODR_M_20HZ 0x14 160 | #define BITS_ODR_M_40HZ 0x18 161 | #define BITS_ODR_M_80HZ 0x1C 162 | #define BITS_FS_M_MASK 0x0C 163 | #define BITS_FS_M_4Gs 0x00 164 | #define BITS_FS_M_8Gs 0x20 165 | #define BITS_FS_M_12Gs 0x40 166 | #define BITS_FS_M_16Gs 0x60 167 | #define BITS_MD_CONTINUOUS 0x00 168 | #define BITS_MD_SINGLE 0x01 169 | #define BITS_MD_POWERDOWN 0x02 170 | #define BITS_OMZ_LOW 0x00 171 | #define BITS_OMZ_MEDIUM 0x04 172 | #define BITS_OMZ_HIGH 0x08 173 | #define BITS_OMZ_ULTRA_HIGH 0x0C 174 | -------------------------------------------------------------------------------- /think/Compute.cpp: -------------------------------------------------------------------------------- 1 | #include "Compute.h" 2 | #include "ImageLogger.h" 3 | #include "VideoSource.h" 4 | #include 5 | #include 6 | 7 | 8 | template 9 | static inline T square(T x) { 10 | return x * x; 11 | } 12 | 13 | struct LineInfo { 14 | float angle; 15 | float length; 16 | }; 17 | 18 | static bool byAngle(const LineInfo& v, const LineInfo& w) { 19 | return v.angle < w.angle; 20 | } 21 | 22 | void Compute::BackgroundLoop() { 23 | const double CannyThreshold1 = 100.; 24 | const double CannyThreshold2 = 130.; 25 | 26 | const int HoughThreshold = cap->imageWidth * cap->imageHeight / 6500; 27 | const int minLineLength = cap->imageHeight / 3; 28 | const double maxGap = 25; 29 | const double rho = 2., theta = 0.02; 30 | 31 | // the matrix produced by birdeye program 32 | static double Hdata[3*3] = { 33 | 0.6675041881879467, -0.6301258503975012, 49.80068082287637, 34 | -0.006612602777747778, 0.7948226618810976, -7.541413704343782, 35 | -8.961567957315749e-05, -0.003982602759291037, 1, 36 | }; 37 | const cv::Mat_ H(3, 3, Hdata); 38 | 39 | const float midLeft = 0.5 * cap->imageWidth; 40 | const float midRight = 0.4 * cap->imageWidth; 41 | 42 | cv::Mat2f line(1, 2); cv::Vec2f* pLine = line[0]; 43 | cv::Mat bw; 44 | 45 | for (int i = 0; ! cap->imagesCaptured.quitting; ++i) { 46 | OutputData out; 47 | out.imageBefore = cap->imagesCaptured.Dequeue(); 48 | if (! out.imageBefore.empty()) { 49 | 50 | // Log every 5th images. 51 | if (log && i % 5 == 0 && log->imagesToLog.size < log->imagesToLog.Capacity()) { 52 | log->imagesToLog.Enqueue(out.imageBefore); 53 | } 54 | 55 | // Convert to grayscale. 56 | cv::cvtColor(out.imageBefore, bw, cv::COLOR_BGR2GRAY); 57 | // Smooth it out. 58 | cv::blur(bw, bw, cv::Size(5,5)); 59 | // Detect edges. 60 | cv::Canny(bw, bw, CannyThreshold1, CannyThreshold2); 61 | // Find line segments. 62 | cv::HoughLinesP(bw, out.lines, rho, theta, HoughThreshold, minLineLength, maxGap); 63 | 64 | std::vector lines; 65 | lines.reserve(out.lines.size()); 66 | for (Lines::iterator pl = out.lines.begin(); pl != out.lines.end(); ++pl) { 67 | cv::Vec4i& l = *pl; 68 | pLine[0] = cv::Vec2d(l[0], l[1]); 69 | pLine[1] = cv::Vec2d(l[2], l[3]); 70 | 71 | // See the line from bird's-eye view. 72 | cv::perspectiveTransform(line, line, H); 73 | 74 | // Make sure pLine[0].y <= pLine[1].y 75 | // Smaller y => the point is closer to the bottom of the image. 76 | if (pLine[0][1] > pLine[1][1]) { 77 | cv::Vec2f v = pLine[0]; 78 | pLine[0] = pLine[1]; 79 | pLine[1] = v; 80 | } 81 | 82 | LineInfo info; 83 | info.length = sqrt(square(pLine[0][0] - pLine[1][0]) + square(pLine[0][1] - pLine[1][1])); 84 | // Estimate the steering angle needed to avoid this line. 85 | // If the line occupies only one side of the image and points outward, 86 | // set the steering angle to 0, to ignore the sideline and go staight ahead. 87 | info.angle = midLeft <= pLine[0][0] && pLine[0][0] <= pLine[1][0] 88 | || midRight >= pLine[0][0] && pLine[0][0] >= pLine[1][0] 89 | ? 0. : M_PI_2 - atan2(pLine[1][1], pLine[1][0] - pLine[0][0]); 90 | lines.push_back(info); 91 | } 92 | if (lines.empty()) { 93 | ++countFramesWithoutLines; 94 | out.angle = 0.; 95 | } 96 | else { 97 | countFramesWithoutLines = 0; 98 | 99 | // Sort the lines by angle, then group them in clusters. 100 | std::sort(lines.begin(), lines.end(), byAngle); 101 | 102 | std::vector clusters; 103 | std::vector::const_iterator l = lines.begin(); 104 | LineInfo clusterInfo; 105 | clusterInfo.angle = (*l).length * (*l).angle; 106 | clusterInfo.length = (*l).length; 107 | while (++l != lines.end()) { 108 | if ((*l).angle - clusterInfo.angle/clusterInfo.length > 0.25) { 109 | clusters.push_back(clusterInfo); 110 | clusterInfo.angle = 0.; 111 | clusterInfo.length = 0.; 112 | } 113 | clusterInfo.angle += (*l).length * (*l).angle; 114 | clusterInfo.length += (*l).length; 115 | } 116 | 117 | // Pick the steering angle from the cluster with most lines (by length). 118 | for (std::vector::const_iterator l = clusters.begin(); l != clusters.end(); ++l) { 119 | if (clusterInfo.length < (*l).length) { 120 | clusterInfo = *l; 121 | } 122 | } 123 | out.angle = clusterInfo.angle/clusterInfo.length; 124 | } 125 | 126 | if (outputPic) { 127 | // Uncomment one of the sections of the code to look at the images at various stages. 128 | #if 1 129 | // Look at the image as captured. 130 | out.imageAfter = out.imageBefore; 131 | #elif 1 132 | // Look at the image from bird's-eye view. 133 | cv::warpPerspective(out.imageBefore, out.imageAfter, H, out.imageBefore.size()); 134 | #else 135 | // Look at the edges in the image. 136 | static const int toGray[3*2] = {0,0, 0,1, 0,2}; 137 | out.imageAfter.create(bw.rows, bw.cols, CV_8UC3); 138 | cv::mixChannels(&bw, 1, &out.imageAfter, 1, toGray, 3); 139 | #endif 140 | } 141 | 142 | out.countFramesWithoutLines = countFramesWithoutLines; 143 | SwapOutputData(out); 144 | } 145 | } 146 | } 147 | 148 | static void* ComputeThread(void* compute) { 149 | ((Compute*)compute)->BackgroundLoop(); 150 | return 0; 151 | } 152 | 153 | void Compute::Start() { 154 | pthread_create(&thread, NULL, ComputeThread, this); 155 | } 156 | 157 | void Compute::Stop() { 158 | cap->imagesCaptured.Quit(); 159 | pthread_join(thread, NULL); 160 | } 161 | 162 | Compute::Compute(VideoSource* c, ImageLogger* lg, bool outPic) 163 | : cap(c) 164 | , log(lg) 165 | , outputPic(outPic) 166 | , countFramesWithoutLines(0) 167 | { 168 | pthread_mutex_init(&dataMutex, NULL); 169 | } 170 | 171 | Compute::~Compute() { 172 | pthread_mutex_destroy(&dataMutex); 173 | } 174 | 175 | void Compute::SwapOutputData(OutputData& data) { 176 | OutputData x = data; 177 | 178 | pthread_mutex_lock(&dataMutex); 179 | data = outputData; 180 | outputData = x; 181 | pthread_mutex_unlock(&dataMutex); 182 | } 183 | -------------------------------------------------------------------------------- /drive/Navio/PCA9685.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | PCA9685 driver code is placed under the BSD license. 3 | Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) 4 | Copyright (c) 2014, Emlid Limited, www.emlid.com 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "PCA9685.h" 31 | 32 | /** PCA9685 constructor. 33 | * @param address I2C address 34 | * @see PCA9685_DEFAULT_ADDRESS 35 | */ 36 | PCA9685::PCA9685(uint8_t address) { 37 | this->devAddr = address; 38 | } 39 | 40 | /** Power on and prepare for general usage. 41 | * This method reads prescale value stored in PCA9685 and calculate frequency based on it. 42 | * Then it enables auto-increment of register address to allow for faster writes. 43 | * And finally the restart is performed to enable clocking. 44 | */ 45 | void PCA9685::initialize() { 46 | this->frequency = getFrequency(); 47 | I2Cdev::writeBit(devAddr, PCA9685_RA_MODE1, PCA9685_MODE1_AI_BIT, 1); 48 | restart(); 49 | } 50 | 51 | /** Verify the I2C connection. 52 | * @return True if connection is valid, false otherwise 53 | */ 54 | bool PCA9685::testConnection() { 55 | uint8_t data; 56 | int8_t status = I2Cdev::readByte(devAddr, PCA9685_RA_PRE_SCALE, &data); 57 | if (status > 0) 58 | return true; 59 | else 60 | return false; 61 | } 62 | 63 | /** Put PCA9685 to sleep mode thus turning off the outputs. 64 | * @see PCA9685_MODE1_SLEEP_BIT 65 | */ 66 | void PCA9685::sleep() { 67 | I2Cdev::writeBit(devAddr, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT, 1); 68 | } 69 | 70 | /** Disable sleep mode and start the outputs. 71 | * @see PCA9685_MODE1_SLEEP_BIT 72 | * @see PCA9685_MODE1_RESTART_BIT 73 | */ 74 | void PCA9685::restart() { 75 | I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, (1 << PCA9685_MODE1_SLEEP_BIT)); 76 | I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, ((1 << PCA9685_MODE1_SLEEP_BIT) 77 | | (1 << PCA9685_MODE1_EXTCLK_BIT))); 78 | I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, ((1 << PCA9685_MODE1_RESTART_BIT) 79 | | (1 << PCA9685_MODE1_EXTCLK_BIT) 80 | | (1 << PCA9685_MODE1_AI_BIT))); 81 | } 82 | 83 | /** Calculate prescale value based on the specified frequency and write it to the device. 84 | * @return Frequency in Hz 85 | * @see PCA9685_RA_PRE_SCALE 86 | */ 87 | float PCA9685::getFrequency() { 88 | uint8_t data; 89 | I2Cdev::readByte(devAddr, PCA9685_RA_PRE_SCALE, &data); 90 | return 24576000.f / 4096.f / (data + 1); 91 | } 92 | 93 | 94 | /** Calculate prescale value based on the specified frequency and write it to the device. 95 | * @param Frequency in Hz 96 | * @see PCA9685_RA_PRE_SCALE 97 | */ 98 | void PCA9685::setFrequency(float frequency) { 99 | sleep(); 100 | usleep(10000); 101 | uint8_t prescale = roundf(24576000.f / 4096.f / frequency) - 1; 102 | I2Cdev::writeByte(devAddr, PCA9685_RA_PRE_SCALE, prescale); 103 | this->frequency = getFrequency(); 104 | restart(); 105 | } 106 | 107 | /** Set channel start offset of the pulse and it's length 108 | * @param Channel number (0-15) 109 | * @param Offset (0-4095) 110 | * @param Length (0-4095) 111 | * @see PCA9685_RA_LED0_ON_L 112 | */ 113 | void PCA9685::setPWM(uint8_t channel, uint16_t offset, uint16_t length) { 114 | uint8_t data[4] = {0, 0, 0, 0}; 115 | if(length == 0) { 116 | data[3] = 0x10; 117 | } else if(length >= 4096) { 118 | data[1] = 0x10; 119 | } else { 120 | data[0] = offset & 0xFF; 121 | data[1] = offset >> 8; 122 | data[2] = length & 0xFF; 123 | data[3] = length >> 8; 124 | } 125 | I2Cdev::writeBytes(devAddr, PCA9685_RA_LED0_ON_L + 4 * channel, 4, data); 126 | } 127 | 128 | /** Set channel's pulse length 129 | * @param Channel number (0-15) 130 | * @param Length (0-4095) 131 | * @see PCA9685_RA_LED0_ON_L 132 | */ 133 | void PCA9685::setPWM(uint8_t channel, uint16_t length) { 134 | setPWM(channel, 0, length); 135 | } 136 | 137 | /** Set channel's pulse length in milliseconds 138 | * @param Channel number (0-15) 139 | * @param Length in milliseconds 140 | * @see PCA9685_RA_LED0_ON_L 141 | */ 142 | void PCA9685::setPWMmS(uint8_t channel, float length_mS) { 143 | setPWM(channel, round((length_mS * 4096.f) / (1000.f / frequency))); 144 | } 145 | 146 | /** Set channel's pulse length in microseconds 147 | * @param Channel number (0-15) 148 | * @param Length in microseconds 149 | * @see PCA9685_RA_LED0_ON_L 150 | */ 151 | void PCA9685::setPWMuS(uint8_t channel, float length_uS) { 152 | setPWM(channel, round((length_uS * 4096.f) / (1000000.f / frequency))); 153 | } 154 | 155 | /** Set start offset of the pulse and it's length for all channels 156 | * @param Offset (0-4095) 157 | * @param Length (0-4095) 158 | * @see PCA9685_RA_ALL_LED_ON_L 159 | */ 160 | void PCA9685::setAllPWM(uint16_t offset, uint16_t length) { 161 | uint8_t data[4] = {offset & 0xFF, offset >> 8, length & 0xFF, length >> 8}; 162 | I2Cdev::writeBytes(devAddr, PCA9685_RA_ALL_LED_ON_L, 4, data); 163 | } 164 | 165 | /** Set pulse length for all channels 166 | * @param Length (0-4095) 167 | * @see PCA9685_RA_ALL_LED_ON_L 168 | */ 169 | void PCA9685::setAllPWM(uint16_t length) { 170 | setAllPWM(0, length); 171 | } 172 | 173 | /** Set pulse length in milliseconds for all channels 174 | * @param Length in milliseconds 175 | * @see PCA9685_RA_ALL_LED_ON_L 176 | */ 177 | void PCA9685::setAllPWMmS(float length_mS) { 178 | setAllPWM(round((length_mS * 4096.f) / (1000.f / frequency))); 179 | } 180 | 181 | /** Set pulse length in microseconds for all channels 182 | * @param Length in microseconds 183 | * @see PCA9685_RA_ALL_LED_ON_L 184 | */ 185 | void PCA9685::setAllPWMuS(float length_uS) { 186 | setAllPWM(round((length_uS * 4096.f) / (1000000.f / frequency))); 187 | } 188 | -------------------------------------------------------------------------------- /look/main.cpp: -------------------------------------------------------------------------------- 1 | #include "Capture.h" 2 | #include "Compute.h" 3 | #include "ImageView.h" 4 | #include "Timer.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | 12 | 13 | static VideoSource* cap = NULL; 14 | static Compute* compute = NULL; 15 | static OutputData latestData; 16 | 17 | static int viewWidth, viewHeight; 18 | static ImageView* view = NULL; 19 | static Mouse mouse; 20 | static bool mouseMoved = false; 21 | 22 | static const int refreshRate = 30; 23 | static Timer timer; 24 | 25 | static GLsync rendering = 0; 26 | 27 | static void DrawData(const OutputData& data) { 28 | view->Draw(data, viewWidth, viewHeight, mouse); 29 | 30 | glDeleteSync(rendering); 31 | rendering = glFenceSync(GL_SYNC_GPU_COMMANDS_COMPLETE, 0); 32 | 33 | glutSwapBuffers(); 34 | 35 | mouseMoved = false; 36 | } 37 | 38 | static void Display() { 39 | if (! rendering) { 40 | OutputData newData; 41 | compute->SwapOutputData(newData); 42 | if (! newData.imageAfter.empty()) { 43 | DrawData(newData); 44 | latestData = newData; 45 | } 46 | else if (mouseMoved && ! cap->playing && ! latestData.imageAfter.empty()) { 47 | DrawData(latestData); 48 | } 49 | } 50 | } 51 | 52 | static void Refresh(int what) { 53 | if (rendering) { 54 | switch (glClientWaitSync(rendering, 0, 1000000000/refreshRate)) { 55 | case GL_ALREADY_SIGNALED: 56 | case GL_CONDITION_SATISFIED: 57 | glDeleteSync(rendering); 58 | rendering = 0; 59 | break; 60 | } 61 | } 62 | 63 | if (rendering) { 64 | glutTimerFunc(0, Refresh, what); 65 | } else { 66 | Display(); 67 | glutTimerFunc(timer.NextSleep(refreshRate,INT_MAX)/1000, Refresh, what); 68 | } 69 | } 70 | 71 | static void LookAtPoint(const cv::Mat& image, int x, int y) { 72 | cv::Mat p = cv::Mat(image, cv::Rect(x, y, 1, 1)).clone(); 73 | std::cerr << "R:" << (int)p.data[2] << " G:" << (int)p.data[1] << " B:" << (int)p.data[0] << std::endl; 74 | cv::cvtColor(p, p, cv::COLOR_BGR2Lab); 75 | std::cerr << "L:" << (int)p.data[0] << " a:" << (int)p.data[1] << " b:" << (int)p.data[2] << std::endl; 76 | std::cerr << std::endl; 77 | } 78 | 79 | static void LookAtRect(const cv::Mat& image, int x0, int y0, int x1, int y1) { 80 | cv::Mat inp(image, cv::Rect(std::min(x0,x1), std::min(y0,y1), std::abs(x0-x1), std::abs(y0-y1))); 81 | if (! inp.empty()) { 82 | cv::Mat m, lab[3]; 83 | cv::cvtColor(inp, m, cv::COLOR_BGR2Lab); 84 | cv::split(m, lab); 85 | double otsu_L = cv::threshold(lab[0], m, 128, 255, cv::THRESH_OTSU); 86 | double otsu_a = cv::threshold(lab[1], m, 128, 255, cv::THRESH_OTSU); 87 | double otsu_b = cv::threshold(lab[2], m, 128, 255, cv::THRESH_OTSU); 88 | double tri_L = cv::threshold(lab[0], m, 128, 255, cv::THRESH_TRIANGLE); 89 | double tri_a = cv::threshold(lab[1], m, 128, 255, cv::THRESH_TRIANGLE); 90 | double tri_b = cv::threshold(lab[2], m, 128, 255, cv::THRESH_TRIANGLE); 91 | std::cerr << "Otsu threshold L:" << otsu_L << " a:" << otsu_a << " b:" << otsu_b << std::endl; 92 | std::cerr << "Triangle threshold L:" << tri_L << " a:" << tri_a << " b:" << tri_b << std::endl; 93 | std::cerr << std::endl; 94 | } 95 | } 96 | 97 | static void MouseDrag(int x, int y) { 98 | mouse.x1 = x; 99 | mouse.y1 = y; 100 | mouseMoved = true; 101 | } 102 | 103 | static void MouseClick(int /*button*/, int state, int x, int y) { 104 | switch (state) { 105 | case GLUT_DOWN: 106 | mouse.down = true; 107 | mouse.x0 = mouse.x1 = x; 108 | mouse.y0 = mouse.y1 = y; 109 | 110 | if (! latestData.imageBefore.empty()) { 111 | LookAtPoint(latestData.imageBefore, x, y); 112 | } 113 | break; 114 | 115 | case GLUT_UP: 116 | mouse.down = false; 117 | 118 | if (! latestData.imageBefore.empty() && x != mouse.x0 && y != mouse.y0) { 119 | LookAtRect(latestData.imageBefore, mouse.x0, mouse.y0, x, y); 120 | } 121 | break; 122 | } 123 | 124 | mouseMoved = true; 125 | } 126 | 127 | static void Keyboard(unsigned char key, int /*x*/, int /*y*/) { 128 | if (key == ' ') { 129 | cap->commands.Enqueue(cap->playing ? VideoSource::Pause : VideoSource::Play); 130 | } 131 | } 132 | 133 | static void SpecialKey(int key, int /*x*/, int /*y*/) { 134 | switch (key) { 135 | case GLUT_KEY_UP: 136 | cap->commands.Enqueue(VideoSource::Rewind); 137 | break; 138 | case GLUT_KEY_LEFT: 139 | cap->commands.Enqueue(VideoSource::PrevFrame); 140 | break; 141 | case GLUT_KEY_RIGHT: 142 | cap->commands.Enqueue(VideoSource::NextFrame); 143 | break; 144 | } 145 | } 146 | 147 | static void Cleanup() { 148 | compute->Stop(); 149 | cap->Stop(); 150 | 151 | delete view; view = NULL; 152 | delete compute; compute = NULL; 153 | delete cap; cap = NULL; 154 | glDeleteSync(rendering); rendering = 0; 155 | } 156 | 157 | static char* LatestRoverImages() { 158 | char roverImagesDir[150]; 159 | sprintf(roverImagesDir, "%s/rover-images", getenv("HOME")); 160 | 161 | struct dirent** dirs = NULL; 162 | const int countDirs = scandir(roverImagesDir, &dirs, NULL, alphasort); 163 | static char fname[200]; 164 | sprintf(fname, "%s/%s/%%04d.png", roverImagesDir, dirs[countDirs-1]->d_name); 165 | 166 | for (int i = 0; i < countDirs; ++i) { 167 | free(dirs[i]); 168 | } 169 | free(dirs); 170 | 171 | return fname; 172 | } 173 | 174 | static void Init(int& argc, char**argv) { 175 | std::string home = getenv("HOME"); 176 | 177 | // Uncomment one of these for capture lines to select the video source. 178 | 179 | // Replay the latest image log from directory rover-images/ 180 | //cap = new Capture(LatestRoverImages(), cv::CAP_IMAGES); 181 | 182 | // Start the replay at the exact frame. 183 | cap = new Capture(home + "/rover-images/2017-01-21-race/0630.png", cv::CAP_IMAGES); 184 | 185 | // Display one single frame. 186 | //cap = new Capture(home + "/rover-images/2017-01-21-race/0851.png", cv::CAP_FFMPEG); 187 | 188 | // Display directly from the camera live. 189 | //cap = new Capture(0); 190 | 191 | 192 | viewWidth = cap->imageWidth; 193 | viewHeight = cap->imageHeight; 194 | 195 | glutInit(&argc, argv); 196 | glutInitDisplayMode(GLUT_RGBA | GLUT_DOUBLE); 197 | glutInitWindowSize(viewWidth, viewHeight); 198 | glutCreateWindow("Windshield"); 199 | glewInit(); 200 | 201 | view = new ImageView(cap->imageWidth, cap->imageHeight); 202 | compute = new Compute(cap, NULL, true); 203 | 204 | cap->Start(); 205 | compute->Start(); 206 | 207 | glutTimerFunc(0, Refresh, 0); 208 | glutDisplayFunc(Display); 209 | glutMouseFunc(MouseClick); 210 | glutMotionFunc(MouseDrag); 211 | glutKeyboardFunc(Keyboard); 212 | glutSpecialFunc(SpecialKey); 213 | glutCloseFunc(Cleanup); 214 | 215 | timer.Start(); 216 | } 217 | 218 | int main(int argc, char**argv) { 219 | Init(argc, argv); 220 | glutMainLoop(); 221 | return 0; 222 | } 223 | -------------------------------------------------------------------------------- /drive/Navio/ADS1115.h: -------------------------------------------------------------------------------- 1 | /* 2 | ADS1115 driver code is placed under the BSD license. 3 | Copyright (c) 2014, Emlid Limited, www.emlid.com 4 | Written by Georgii Staroselskii 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #ifndef _ADS1115_H_ 31 | #define _ADS1115_H_ 32 | 33 | #include 34 | 35 | #ifdef DEBUG_ADS1115 36 | #define debug(M, ...) fprintf(stderr, "DEBUG %s:%d: " M "\n", __FILE__, __LINE__, ##__VA_ARGS__) 37 | #else 38 | #define debug(M, ...) 39 | #endif 40 | 41 | #define ADS1115_ADDRESS_ADDR_GND 0x48 // address pin low (GND) 42 | #define ADS1115_ADDRESS_ADDR_VDD 0x49 // address pin high (VCC) 43 | #define ADS1115_ADDRESS_ADDR_SDA 0x4A // address pin tied to SDA pin 44 | #define ADS1115_ADDRESS_ADDR_SCL 0x4B // address pin tied to SCL pin 45 | #define ADS1115_DEFAULT_ADDRESS ADS1115_ADDRESS_ADDR_GND 46 | 47 | #define ADS1115_RA_CONVERSION 0x00 48 | #define ADS1115_RA_CONFIG 0x01 49 | #define ADS1115_RA_LO_THRESH 0x02 50 | #define ADS1115_RA_HI_THRESH 0x03 51 | 52 | #define ADS1115_OS_SHIFT 15 53 | #define ADS1115_OS_INACTIVE 0x00 << ADS1115_OS_SHIFT 54 | #define ADS1115_OS_ACTIVE 0x01 << ADS1115_OS_SHIFT 55 | 56 | #define ADS1115_MUX_SHIFT 12 57 | #define ADS1115_MUX_P0_N1 0x00 << ADS1115_MUX_SHIFT /* default */ 58 | #define ADS1115_MUX_P0_N3 0x01 << ADS1115_MUX_SHIFT 59 | #define ADS1115_MUX_P1_N3 0x02 << ADS1115_MUX_SHIFT 60 | #define ADS1115_MUX_P2_N3 0x03 << ADS1115_MUX_SHIFT 61 | #define ADS1115_MUX_P0_NG 0x04 << ADS1115_MUX_SHIFT 62 | #define ADS1115_MUX_P1_NG 0x05 << ADS1115_MUX_SHIFT 63 | #define ADS1115_MUX_P2_NG 0x06 << ADS1115_MUX_SHIFT 64 | #define ADS1115_MUX_P3_NG 0x07 << ADS1115_MUX_SHIFT 65 | 66 | #define ADS1115_PGA_SHIFT 9 67 | #define ADS1115_PGA_6P144 0x00 << ADS1115_PGA_SHIFT 68 | #define ADS1115_PGA_4P096 0x01 << ADS1115_PGA_SHIFT 69 | #define ADS1115_PGA_2P048 0x02 << ADS1115_PGA_SHIFT // default 70 | #define ADS1115_PGA_1P024 0x03 << ADS1115_PGA_SHIFT 71 | #define ADS1115_PGA_0P512 0x04 << ADS1115_PGA_SHIFT 72 | #define ADS1115_PGA_0P256 0x05 << ADS1115_PGA_SHIFT 73 | #define ADS1115_PGA_0P256B 0x06 << ADS1115_PGA_SHIFT 74 | #define ADS1115_PGA_0P256C 0x07 << ADS1115_PGA_SHIFT 75 | 76 | #define ADS1115_MV_6P144 0.187500 77 | #define ADS1115_MV_4P096 0.125000 78 | #define ADS1115_MV_2P048 0.062500 // default 79 | #define ADS1115_MV_1P024 0.031250 80 | #define ADS1115_MV_0P512 0.015625 81 | #define ADS1115_MV_0P256 0.007813 82 | #define ADS1115_MV_0P256B 0.007813 83 | #define ADS1115_MV_0P256C 0.007813 84 | 85 | #define ADS1115_MODE_SHIFT 8 86 | #define ADS1115_MODE_CONTINUOUS 0x00 << ADS1115_MODE_SHIFT 87 | #define ADS1115_MODE_SINGLESHOT 0x01 << ADS1115_MODE_SHIFT // default 88 | 89 | #define ADS1115_RATE_SHIFT 5 90 | #define ADS1115_RATE_8 0x00 << ADS1115_RATE_SHIFT 91 | #define ADS1115_RATE_16 0x01 << ADS1115_RATE_SHIFT 92 | #define ADS1115_RATE_32 0x02 << ADS1115_RATE_SHIFT 93 | #define ADS1115_RATE_64 0x03 << ADS1115_RATE_SHIFT 94 | #define ADS1115_RATE_128 0x04 << ADS1115_RATE_SHIFT // default 95 | #define ADS1115_RATE_250 0x05 << ADS1115_RATE_SHIFT 96 | #define ADS1115_RATE_475 0x06 << ADS1115_RATE_SHIFT 97 | #define ADS1115_RATE_860 0x07 << ADS1115_RATE_SHIFT 98 | 99 | #define ADS1115_COMP_MODE_SHIFT 4 100 | #define ADS1115_COMP_MODE_HYSTERESIS 0x00 << ADS1115_COMP_MODE_SHIFT // default 101 | #define ADS1115_COMP_MODE_WINDOW 0x01 << ADS1115_COMP_MODE_SHIFT 102 | 103 | #define ADS1115_COMP_POL_SHIFT 3 104 | #define ADS1115_COMP_POL_ACTIVE_LOW 0x00 << ADS1115_COMP_POL_SHIFT // default 105 | #define ADS1115_COMP_POL_ACTIVE_HIGH 0x01 << ADS1115_COMP_POL_SHIFT 106 | 107 | #define ADS1115_COMP_LAT_SHIFT 2 108 | #define ADS1115_COMP_LAT_NON_LATCHING 0x00 << ADS1115_COMP_LAT_SHIFT // default 109 | #define ADS1115_COMP_LAT_LATCHING 0x01 << ADS1115_COMP_LAT_SHIFT 110 | 111 | #define ADS1115_COMP_QUE_SHIFT 0 112 | #define ADS1115_COMP_QUE_ASSERT1 0x00 << ADS1115_COMP_SHIFT 113 | #define ADS1115_COMP_QUE_ASSERT2 0x01 << ADS1115_COMP_SHIFT 114 | #define ADS1115_COMP_QUE_ASSERT4 0x02 << ADS1115_COMP_SHIFT 115 | #define ADS1115_COMP_QUE_DISABLE 0x03 // default 116 | 117 | #include "I2Cdev.h" 118 | 119 | class ADS1115 { 120 | public: 121 | ADS1115(uint8_t address = ADS1115_DEFAULT_ADDRESS); 122 | ~ADS1115(); 123 | 124 | bool testConnection(); 125 | 126 | int16_t getConversion(); 127 | 128 | void setOpStatus(uint16_t op); 129 | 130 | uint16_t getMultiplexer(); 131 | void setMultiplexer(uint16_t mux); 132 | 133 | uint16_t getGain(); 134 | void setGain(uint16_t gain); 135 | 136 | uint16_t getMode(); 137 | void setMode(uint16_t mode); 138 | 139 | uint16_t getRate(); 140 | void setRate(uint16_t rate); 141 | 142 | float getMilliVolts(); 143 | 144 | void setComparatorMode(uint16_t comparatorMode); 145 | void setComparatorPolarity(uint16_t polarit); 146 | void setComparatorLatchEnabled(uint16_t latchStatus); 147 | void setComparatorQueueMode(uint16_t queueMode); 148 | 149 | private: 150 | void updateConfigRegister(); 151 | 152 | uint8_t address; 153 | 154 | struct configRegister { 155 | uint16_t status; 156 | uint16_t mux; 157 | uint16_t gain; 158 | uint16_t mode; 159 | uint16_t rate; 160 | uint16_t comparator; 161 | uint16_t polarity; 162 | uint16_t latch; 163 | uint16_t queue; 164 | } config; 165 | 166 | void showConfigRegister(); 167 | }; 168 | 169 | #endif /* _ADS1115_H_ */ 170 | -------------------------------------------------------------------------------- /drive/Navio/LSM9DS1.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Written by Alexey Bulatov (alexey.bulatov@emlid.com) for Raspberry Pi 3 | */ 4 | 5 | #include "LSM9DS1.h" 6 | 7 | #define DEVICE_ACC_GYRO "/dev/spidev0.3" 8 | #define DEVICE_MAGNETOMETER "/dev/spidev0.2" 9 | 10 | #define READ_FLAG 0x80 11 | #define MULTIPLE_READ 0x40 12 | 13 | #define G_SI 9.80665 14 | #define PI 3.14159 15 | 16 | LSM9DS1::LSM9DS1() 17 | { 18 | } 19 | 20 | /*----------------------------------------------------------------------------------------------- 21 | REGISTER READ & WRITE 22 | usage: use these methods to read and write LSM9DS1 registers over SPI 23 | -----------------------------------------------------------------------------------------------*/ 24 | 25 | unsigned int LSM9DS1::WriteReg(const char *dev, uint8_t WriteAddr, uint8_t WriteData ) 26 | { 27 | unsigned char tx[2] = {WriteAddr, WriteData}; 28 | unsigned char rx[2] = {0}; 29 | SPIdev::transfer(dev, tx, rx, 2); 30 | return rx[1]; 31 | } 32 | 33 | unsigned int LSM9DS1::ReadReg(const char *dev, uint8_t ReadAddr) 34 | { 35 | return WriteReg(dev, ReadAddr | READ_FLAG, 0x00); 36 | } 37 | 38 | void LSM9DS1::ReadRegs(const char *dev, uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes ) 39 | { 40 | unsigned char tx[255] = {0}; 41 | unsigned char rx[255] = {0}; 42 | 43 | tx[0] = ReadAddr | READ_FLAG; 44 | if (!strcmp(dev, DEVICE_MAGNETOMETER)) tx[0] |= MULTIPLE_READ; 45 | 46 | SPIdev::transfer(dev, tx, rx, Bytes + 1); 47 | 48 | for (uint i = 0; i < Bytes; i++) 49 | ReadBuf[i] = rx[i + 1]; 50 | 51 | usleep(50); 52 | } 53 | 54 | /*----------------------------------------------------------------------------------------------- 55 | TEST CONNECTION 56 | usage: call this function to know if SPI and LSM9DS1 are working correctly. 57 | returns true if Accel/Gyro and Magnetometer answers 58 | -----------------------------------------------------------------------------------------------*/ 59 | 60 | bool LSM9DS1::probe() 61 | { 62 | uint8_t responseXG,responseM; 63 | responseXG = ReadReg(DEVICE_ACC_GYRO, LSM9DS1XG_WHO_AM_I); 64 | responseM = ReadReg(DEVICE_MAGNETOMETER, LSM9DS1M_WHO_AM_I); 65 | if (responseXG == WHO_AM_I_ACC_GYRO && responseM == WHO_AM_I_MAG) 66 | return true; 67 | else 68 | return false; 69 | } 70 | 71 | bool LSM9DS1::initialize() 72 | { 73 | //--------Accelerometer and Gyroscope--------- 74 | // enable the 3-axes of the gyroscope 75 | WriteReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG4, BITS_XEN_G | 76 | BITS_YEN_G | 77 | BITS_ZEN_G); 78 | // configure the gyroscope 79 | WriteReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG1_G, BITS_ODR_G_952HZ | 80 | BITS_FS_G_2000DPS); 81 | usleep(200); 82 | 83 | // enable the three axes of the accelerometer 84 | WriteReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG5_XL, BITS_XEN_XL | 85 | BITS_YEN_XL | 86 | BITS_ZEN_XL); 87 | // configure the accelerometer-specify bandwidth selection with Abw 88 | WriteReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG6_XL, BITS_ODR_XL_952HZ | 89 | BITS_FS_XL_16G); 90 | usleep(200); 91 | 92 | //------------Magnetometer---------------- 93 | WriteReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG1_M, BITS_TEMP_COMP | 94 | BITS_OM_HIGH | 95 | BITS_ODR_M_80HZ); 96 | WriteReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG2_M, BITS_FS_M_16Gs); 97 | // continuous conversion mode 98 | WriteReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG3_M, BITS_MD_CONTINUOUS); 99 | WriteReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG4_M, BITS_OMZ_HIGH); 100 | WriteReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG5_M, 0x00 ); 101 | usleep(200); 102 | 103 | set_gyro_scale(BITS_FS_G_2000DPS); 104 | set_acc_scale(BITS_FS_XL_16G); 105 | set_mag_scale(BITS_FS_M_16Gs); 106 | return true; 107 | } 108 | 109 | void LSM9DS1::update() 110 | { 111 | uint8_t response[6]; 112 | int16_t bit_data[3]; 113 | 114 | // Read temperature 115 | ReadRegs(DEVICE_ACC_GYRO, LSM9DS1XG_OUT_TEMP_L, &response[0], 2); 116 | temperature = (float)(((int16_t)response[1] << 8) | response[0]) / 256. + 25.; 117 | 118 | // Read accelerometer 119 | ReadRegs(DEVICE_ACC_GYRO, LSM9DS1XG_OUT_X_L_XL, &response[0], 6); 120 | for (int i=0; i<3; i++) { 121 | bit_data[i] = ((int16_t)response[2*i+1] << 8) | response[2*i] ; 122 | } 123 | _ax = G_SI * ((float)bit_data[0] * acc_scale); 124 | _ay = G_SI * ((float)bit_data[1] * acc_scale); 125 | _az = G_SI * ((float)bit_data[2] * acc_scale); 126 | 127 | // Read gyroscope 128 | ReadRegs(DEVICE_ACC_GYRO, LSM9DS1XG_OUT_X_L_G, &response[0], 6); 129 | for (int i=0; i<3; i++) { 130 | bit_data[i] = ((int16_t)response[2*i+1] << 8) | response[2*i] ; 131 | } 132 | _gx = (PI / 180) * ((float)bit_data[0] * gyro_scale); 133 | _gy = (PI / 180) * ((float)bit_data[1] * gyro_scale); 134 | _gz = (PI / 180) * ((float)bit_data[2] * gyro_scale); 135 | 136 | // Read magnetometer 137 | ReadRegs(DEVICE_MAGNETOMETER, LSM9DS1M_OUT_X_L_M, &response[0], 6); 138 | for (int i=0; i<3; i++) { 139 | bit_data[i] = ((int16_t)response[2*i+1] << 8) | response[2*i] ; 140 | } 141 | _mx = 100.0 * ((float)bit_data[0] * mag_scale); 142 | _my = 100.0 * ((float)bit_data[1] * mag_scale); 143 | _mz = 100.0 * ((float)bit_data[2] * mag_scale); 144 | 145 | // Change rotation of LSM9DS1 like in MPU-9250 146 | rotate(); 147 | } 148 | 149 | void LSM9DS1::rotate() 150 | { 151 | float replacement_acc, replacement_gyro; 152 | 153 | replacement_acc = _ax; 154 | _ax = -_ay; 155 | _ay = -replacement_acc; 156 | 157 | replacement_gyro = _gx; 158 | _gx = -_gy; 159 | _gy = -replacement_gyro; 160 | 161 | _my = -_my; 162 | _mz = -_mz; 163 | } 164 | 165 | void LSM9DS1::set_gyro_scale(int scale) 166 | { 167 | uint8_t reg; 168 | reg = BITS_FS_G_MASK & ReadReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG1_G); 169 | WriteReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG1_G,reg | scale); 170 | switch (scale) { 171 | case BITS_FS_G_245DPS: 172 | gyro_scale = 0.00875; 173 | break; 174 | case BITS_FS_G_500DPS: 175 | gyro_scale = 0.0175; 176 | break; 177 | case BITS_FS_G_2000DPS: 178 | gyro_scale = 0.07; 179 | break; 180 | } 181 | } 182 | 183 | void LSM9DS1::set_acc_scale(int scale) 184 | { 185 | uint8_t reg; 186 | reg = BITS_FS_XL_MASK & ReadReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG6_XL); 187 | WriteReg(DEVICE_ACC_GYRO, LSM9DS1XG_CTRL_REG6_XL, reg | scale); 188 | switch (scale) { 189 | case BITS_FS_XL_2G: 190 | acc_scale = 0.000061; 191 | break; 192 | case BITS_FS_XL_4G: 193 | acc_scale = 0.000122; 194 | break; 195 | case BITS_FS_XL_8G: 196 | acc_scale = 0.000244; 197 | break; 198 | case BITS_FS_XL_16G: 199 | acc_scale = 0.000732; 200 | break; 201 | } 202 | } 203 | 204 | void LSM9DS1::set_mag_scale(int scale) 205 | { 206 | uint8_t reg; 207 | reg = BITS_FS_M_MASK & ReadReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG2_M); 208 | WriteReg(DEVICE_MAGNETOMETER, LSM9DS1M_CTRL_REG2_M, reg | scale); 209 | switch (scale) { 210 | case BITS_FS_M_4Gs: 211 | mag_scale = 0.00014; 212 | break; 213 | case BITS_FS_M_8Gs: 214 | mag_scale = 0.00029; 215 | break; 216 | case BITS_FS_M_12Gs: 217 | mag_scale = 0.00043; 218 | break; 219 | case BITS_FS_M_16Gs: 220 | mag_scale = 0.00058; 221 | break; 222 | } 223 | } 224 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Windshield 2 | Windshield is our software for driving a model car on a race course, using only one camera to detect the track. It competed in 3 | the [DIY Robocars race on January 21, 2017] (https://www.meetup.com/DIYRobocars/events/236393485/), and was the only car that 4 | completed a lap at racetime. The race track was [marked with white tapes](https://www.meetup.com/DIYRobocars/photos/27550435/#457707167). 5 | 6 | Windshield consists of three programs: 7 | 8 | * `drive` controls the car. It runs on Raspberry 3 with the camera and Navio2 board to control the motors. 9 | (https://docs.emlid.com/navio2/) 10 | * `look` plays back the video recorded by `drive` for analysis. `drive` records every 5th image frames captured by the camera. 11 | `look` runs on Linux with GUI. 12 | (The directory `think` contains the image processing code, shared by `drive` and `look`.) 13 | * `birdeye` calibrates the camera pose and produces the matrix to tranform the images to bird's-eye view. 14 | 15 | ### Hardware Requirements 16 | The drive program runs on Raspberry 3 with the camera and Navio2 board to control the motors. The drive program does not use 17 | any sensors other than the camera. You can modify `drive/Motor.cpp` and `drive/Radio.cpp` to support other motor cotrol boards. 18 | 19 | As a safety measure, the drive program reads the throttle position of your RC transmitter and use it to set the speed of 20 | the car. You need a RC transmitter and a receiver supported by Navio2. 21 | 22 | Our car was based on one of the [rover kits from from Lynxmotion](http://www.lynxmotion.com/c-30-rovers.aspx). 23 | 24 | ### Software Configuration 25 | Windshield requires OpenCV 3 on both Raspberry Pi 3 and the host computer. 26 | 27 | Windshield is built with `cmake`. On Raspberry Pi 3: 28 | 29 | cd drive 30 | mkdir build 31 | cd build 32 | cmake .. 33 | make 34 | 35 | Do the same thing on the host computer for `look` and `birdeye`. 36 | 37 | ### Calibration 38 | Download the OpenCV chessboard pattern https://github.com/opencv/opencv/blob/master/doc/pattern.png 39 | Print it out. Tape it on the floor in front the car. Take a picture of it from the pi camera using `raspistill` 40 | (https://www.raspberrypi.org/documentation/usage/camera/raspicam/raspistill.md). It should look like `pic3.jpg` in `birdeye` 41 | directory: https://github.com/compound-eye/windshield/blob/master/birdeye/pic3.jpg 42 | 43 | Replace `birdeye/pic3.jpg` with the picture you just took. Run `birdeye`. Copy the output matrix to the variable `Hdata` in 44 | `think/Compute.cpp`. 45 | 46 | If your camera has a different resolution than 320x240, change the variable `dst` in `birdeye.cpp` to reflect that. When you 47 | take the chessboard picture, use the highest resolution available on the camera, and let `birdeye` scale it down. 48 | 49 | ## Drive 50 | On Raspbery Pi 3, create the directory `rover-images` under the home directory. The drive program will save every 5th images 51 | from the camera in `rover-images`. 52 | 53 | Turn on the RC transmitter. Make sure the light on the receiver is on. Move the throttle to the lowest position. 54 | 55 | From `drive` directory, start the program with the command `sudo build/rover`. Raise the throttle. 56 | (The drive program needs su privilege for the access to Navio2.) 57 | 58 | Control-C to stop the drive program. 59 | 60 | Our car uses differential drive, so currently `drive/main.cpp` steers the car by setting the speeds for the left and right 61 | motors. As the output from `think/Compute.cpp` is the steering angle, it should be straightforward to adapt 62 | `drive/main.cpp` for conventional steering. If you use the conventional steering, be sure to add a filter to smooth 63 | out the steering control, or you will wear out the steering servo very fast. 64 | 65 | ### Look at the Video Log 66 | Here are the [images from the successful race lap]( https://github.com/compound-eye/rover-images). 67 | 68 | In `look/main.cpp`, uncomment the suitable line to select the image files for replay. Save, build, and run. 69 | 70 | * space bar: pause/resume 71 | * up arrow: rewind 72 | * left arrow: previous frame 73 | * right arrow: next frame 74 | 75 | In `think/Compute.cpp`, you can comment/uncomment sections of code to display the images at various stage of processing. 76 | 77 | ## Theory 78 | `think/Compute.cpp` processes the captured image and outputs a new steering angle. 79 | 80 | Our goal is simply to drive between the lines. Our algorithm reduces to: 81 | 82 | 83 | WHILE new image { 84 | find lines 85 | IF there is a line crossing our path { 86 | turn parallel to the line to avoid it 87 | } ELSE { 88 | drive straight 89 | } 90 | } 91 | 92 | No, it's not very complicated. KISS. 93 | 94 | Notes: 95 | 96 | 1. The default is to drive straight. If we fail to see the edge of the road, we roll off the track and keep going, until we see something else that looks like a lane marker. 97 | 2. We don’t care about lines to our side that we’re not in danger of crossing; in particular we don’t try to hug our lane. 98 | 99 | ###Finding Lines 100 | 101 | We use a standard technique to find lines in each image. 102 | 103 | * Convert to grayscale 104 | * Blur 105 | * Canny edge detection 106 | * Hough line search 107 | 108 | The Wikipedia articles on [Canny edge detectors](https://en.wikipedia.org/wiki/Canny_edge_detector) and the [Hough transform](https://en.wikipedia.org/wiki/Hough_transform) are good. Here are some of the issues that we ran into: 109 | 110 | Scratches in the concrete floor and reflections of the roof structure in puddles of rain generated lots of spurious edges and lines in our tests. A 5x5 Gaussian blur took care of most of those. We also threw away line segments that were very short or that required connecting widely separated dots; those are tunable parameters in the OpenCV implementation of the Hough transform. 111 | 112 | We had to tune other Canny and Hough thresholds for the environment: wooden floors in brightly lit homes are not the same as concrete floors in dank warehouses. 113 | 114 | The output of these steps is an array of lines, characterized by their start and end points in pixel co-ordinates. Those co-ordinates are not directly useful because of perspective distortion. 115 | 116 | ###Correcting for perspective 117 | 118 | Imagine we’re driving along a straight road that extends to the horizon. From the point of view of a dashcam the lane markers are converging (towards a vanishing point). If the road turns slightly to the right, the left edge will cross our direction of travel, but at a much steeper angle than the steering correction we need to make. 119 | 120 | What we really want is a top down or bird’s eye view of the road and our position. We could synthesize that viewpoint if we had a 3D map of the scene from LIDAR or stereo, but we don’t. 121 | 122 | Instead we assume that the region of interest (the ROI, in this case the track and all the lane markings) is a horizontal plane. Given that assumption and the pose of our camera, we can compute a 3x3 homography that when applied to the image renders a top down view of the ROI. 123 | 124 | We figure out the camera pose ahead of time using a calibration target and the `birdeye` app. 125 | 126 | We don’t need to warp the whole image; we only need to warp the lines that we find. This is very fast. 127 | 128 | Note: 129 | 130 | 1. This technique is called the Inverse Perspective Transform in the literature. 131 | 2. After calibration the camera cannot move with respect to the robot, so no panning or tilting to look for lines. 132 | 3. Outside the ROI the output is geometrically meaningless: walls, spectators, and other cars will not appear as they would in a true top down image. 133 | 4. If the course included steep up or down slopes that invalidated the planar assumption, we'd have some issues. 134 | 135 | ###Choosing a steering angle 136 | 137 | Now we have an array of line segments that mostly correspond to the edges of the track, plus some garbage, as seen from above. 138 | 139 | The remaining steps are: 140 | 141 | * Compute the angle that the line segments make with the horizontal. 142 | * Filter out line segments that appear on one side of the image only and lean outwards, on the assumption that we’re not in danger of crossing those. 143 | * Cluster the rest by angle. 144 | * Find the cluster with the longest combined length, and set that angle as our steering angle. 145 | -------------------------------------------------------------------------------- /drive/Navio/ADS1115.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | ADS1115 driver code is placed under the BSD license. 3 | Copyright (c) 2014, Emlid Limited, www.emlid.com 4 | Written by Georgii Staroselskii 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include "ADS1115.h" 35 | 36 | /** 37 | * @brief ADS1115 constructor 38 | * Default gain is 4096 39 | * Default multiplexer is between GND and P0 40 | * Default comparator mode is disabled 41 | * Default mode is singleshot 42 | * @param address 43 | */ 44 | ADS1115::ADS1115(uint8_t address) { 45 | this->address = address; 46 | memset(&config, 0, sizeof(config)); 47 | setGain(ADS1115_PGA_4P096); 48 | setMultiplexer(ADS1115_MUX_P0_NG); 49 | setMode(ADS1115_MODE_SINGLESHOT); 50 | setComparatorQueueMode(ADS1115_COMP_QUE_DISABLE); 51 | } 52 | 53 | ADS1115::~ADS1115() { 54 | 55 | } 56 | 57 | /** Verify the I2C connection. 58 | * @return True if connection is valid, false otherwise 59 | */ 60 | bool ADS1115::testConnection() { 61 | uint8_t data; 62 | int8_t status = I2Cdev::readByte(address, ADS1115_RA_CONFIG, &data); 63 | if (status > 0) 64 | return true; 65 | else 66 | return false; 67 | } 68 | 69 | /** 70 | * @brief Call it if you updated ConfigRegister 71 | */ 72 | void ADS1115::updateConfigRegister() { 73 | uint16_t c; 74 | 75 | /* New config */ 76 | c = config.status | config.mux | config.gain | 77 | config.mode | config.rate | config.comparator | 78 | config.polarity | config.latch | config.queue; 79 | 80 | if ( I2Cdev::writeWord(address, ADS1115_RA_CONFIG, c) < 0) { 81 | fprintf(stderr, "Error while writing config\n"); 82 | } 83 | } 84 | 85 | /** 86 | * @brief Get data from Conversion Register 87 | * 88 | * @return Little-Endian result 89 | */ 90 | int16_t ADS1115::getConversion() { 91 | union { 92 | uint16_t w; 93 | uint8_t b[2]; 94 | } word; 95 | word.w = 0; 96 | 97 | if (config.mode == ADS1115_MODE_SINGLESHOT ) { 98 | /* Check for Operation Status. If it is 0 then we are ready to get data. Otherwise wait. */ 99 | setOpStatus(ADS1115_OS_ACTIVE); 100 | while ((word.w & 0x80) == 0) { 101 | if ( I2Cdev::readWord(address, ADS1115_RA_CONFIG, &word.w) < 0 ) 102 | fprintf(stderr, "Error while reading config\n"); 103 | } 104 | } 105 | 106 | if ( (I2Cdev::readWord(address, ADS1115_RA_CONVERSION, &word.w)) < 0 ) { 107 | fprintf(stderr, "Error while reading\n"); 108 | } 109 | /* Exchange MSB and LSB */ 110 | word.w = word.b[0] << 8 | word.b[1]; 111 | return (int16_t) word.w; 112 | } 113 | 114 | /** 115 | * @brief Update Operational Status 116 | * 117 | * @param Desired Status 118 | */ 119 | void ADS1115::setOpStatus(uint16_t status) { 120 | config.status = status; 121 | updateConfigRegister(); 122 | } 123 | 124 | /** 125 | * @brief Check which Multiplexer is selected 126 | * 127 | * @return Multiplexer status 128 | */ 129 | uint16_t ADS1115::getMultiplexer() { 130 | return config.mux; 131 | } 132 | 133 | /** 134 | * @brief Choose the multiplexer 135 | * 136 | * @param Desired multiplexer 137 | */ 138 | void ADS1115::setMultiplexer(uint16_t mux) { 139 | if (config.mux != mux) { 140 | config.mux = mux; 141 | updateConfigRegister(); 142 | } 143 | } 144 | 145 | /** 146 | * @brief Get current gain 147 | * 148 | * @return Current Gain 149 | */ 150 | uint16_t ADS1115::getGain() { 151 | return config.gain; 152 | } 153 | 154 | /** 155 | * @brief Set gain 156 | * 157 | * @param gain 158 | */ 159 | void ADS1115::setGain(uint16_t gain) { 160 | if (config.gain != gain) { 161 | config.gain = gain; 162 | updateConfigRegister(); 163 | } 164 | } 165 | 166 | /** 167 | * @brief Get mode 168 | * 169 | * @return mode 170 | */ 171 | uint16_t ADS1115::getMode() { 172 | return config.mode; 173 | } 174 | 175 | /** 176 | * @brief Set mode 177 | * 178 | * @param mode 179 | */ 180 | void ADS1115::setMode(uint16_t mode) { 181 | if (config.mode != mode) { 182 | config.mode = mode; 183 | updateConfigRegister(); 184 | } 185 | } 186 | 187 | /** 188 | * @brief Get rate 189 | * 190 | * @return rate 191 | */ 192 | uint16_t ADS1115::getRate() { 193 | return config.rate; 194 | } 195 | 196 | /** 197 | * @brief Set rate 198 | * 199 | * @param rate 200 | */ 201 | void ADS1115::setRate(uint16_t rate) { 202 | if (config.rate != rate) { 203 | config.rate = rate; 204 | updateConfigRegister(); 205 | } 206 | } 207 | 208 | /** 209 | * @brief Show content of config register 210 | */ 211 | void ADS1115::showConfigRegister() { 212 | union { 213 | uint16_t w; 214 | uint8_t b[2]; 215 | } buf; 216 | I2Cdev::readWord(address, ADS1115_RA_CONFIG, &buf.w); 217 | 218 | debug("Config Register: 0x%04x | 0x%02x 0x%02x", buf.w, buf.b[0], buf.b[1]); 219 | } 220 | 221 | /** 222 | * @brief Get content of conversion register in mV. It gets converted using current gain 223 | * @see setGain 224 | * 225 | * @return Last conversion in mV 226 | */ 227 | float ADS1115::getMilliVolts() { 228 | switch (config.gain) { 229 | case ADS1115_PGA_6P144: 230 | return (getConversion() * ADS1115_MV_6P144); 231 | break; 232 | case ADS1115_PGA_4P096: 233 | return (getConversion() * ADS1115_MV_4P096); 234 | break; 235 | case ADS1115_PGA_2P048: 236 | return (getConversion() * ADS1115_MV_2P048); 237 | break; 238 | case ADS1115_PGA_1P024: 239 | return (getConversion() * ADS1115_MV_1P024); 240 | break; 241 | case ADS1115_PGA_0P512: 242 | return (getConversion() * ADS1115_MV_0P512); 243 | break; 244 | case ADS1115_PGA_0P256: 245 | case ADS1115_PGA_0P256B: 246 | case ADS1115_PGA_0P256C: 247 | return (getConversion() * ADS1115_MV_0P256); 248 | break; 249 | default: 250 | fprintf(stderr, "Wrong gain\n"); 251 | return -1; 252 | break; 253 | } 254 | } 255 | 256 | /** 257 | * @brief set comparator mode 258 | * 259 | * @param comparator 260 | */ 261 | void ADS1115::setComparatorMode(uint16_t comparator) { 262 | if (config.comparator != comparator) { 263 | config.comparator = comparator; 264 | updateConfigRegister(); 265 | } 266 | } 267 | 268 | /** 269 | * @brief Set Comparator polarity 270 | * 271 | * @param polarity 272 | */ 273 | void ADS1115::setComparatorPolarity(uint16_t polarity) { 274 | if (config.polarity != polarity) { 275 | config.polarity = polarity; 276 | updateConfigRegister(); 277 | } 278 | } 279 | 280 | /** 281 | * @brief Set comparator latch status 282 | * 283 | * @param latch 284 | */ 285 | void ADS1115::setComparatorLatchEnabled(uint16_t latch) { 286 | if (config.latch != latch) { 287 | config.latch = latch; 288 | updateConfigRegister(); 289 | } 290 | } 291 | 292 | /** 293 | * @brief Set Comparator Queue Mode 294 | * 295 | * @param queue 296 | */ 297 | void ADS1115::setComparatorQueueMode(uint16_t queue) { 298 | if (config.queue != queue) { 299 | config.queue = queue; 300 | updateConfigRegister(); 301 | } 302 | 303 | } 304 | -------------------------------------------------------------------------------- /drive/Navio/MPU9250.h: -------------------------------------------------------------------------------- 1 | /* 2 | Written by Qiyong Mu (kylongmu@msn.com) 3 | Adapted for Raspberry Pi by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) 4 | */ 5 | 6 | #ifndef _MPU9250_H 7 | #define _MPU9250_H 8 | 9 | #include "SPIdev.h" 10 | #include "InertialSensor.h" 11 | 12 | class MPU9250 : public InertialSensor 13 | { 14 | public: 15 | MPU9250(); 16 | 17 | bool initialize(); 18 | bool probe(); 19 | void update(); 20 | 21 | private: 22 | unsigned int WriteReg(uint8_t WriteAddr, uint8_t WriteData); 23 | unsigned int ReadReg(uint8_t ReadAddr); 24 | void ReadRegs(uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes); 25 | 26 | unsigned int set_gyro_scale(int scale); 27 | unsigned int set_acc_scale(int scale); 28 | 29 | void calib_acc(); 30 | void calib_mag(); 31 | 32 | float acc_divider; 33 | float gyro_divider; 34 | 35 | int calib_data[3]; 36 | float magnetometer_ASA[3]; 37 | }; 38 | 39 | #endif //_MPU9250_H 40 | 41 | // MPU9250 registers 42 | #define MPUREG_XG_OFFS_TC 0x00 43 | #define MPUREG_YG_OFFS_TC 0x01 44 | #define MPUREG_ZG_OFFS_TC 0x02 45 | #define MPUREG_X_FINE_GAIN 0x03 46 | #define MPUREG_Y_FINE_GAIN 0x04 47 | #define MPUREG_Z_FINE_GAIN 0x05 48 | #define MPUREG_XA_OFFS_H 0x06 49 | #define MPUREG_XA_OFFS_L 0x07 50 | #define MPUREG_YA_OFFS_H 0x08 51 | #define MPUREG_YA_OFFS_L 0x09 52 | #define MPUREG_ZA_OFFS_H 0x0A 53 | #define MPUREG_ZA_OFFS_L 0x0B 54 | #define MPUREG_PRODUCT_ID 0x0C 55 | #define MPUREG_SELF_TEST_X 0x0D 56 | #define MPUREG_SELF_TEST_Y 0x0E 57 | #define MPUREG_SELF_TEST_Z 0x0F 58 | #define MPUREG_SELF_TEST_A 0x10 59 | #define MPUREG_XG_OFFS_USRH 0x13 60 | #define MPUREG_XG_OFFS_USRL 0x14 61 | #define MPUREG_YG_OFFS_USRH 0x15 62 | #define MPUREG_YG_OFFS_USRL 0x16 63 | #define MPUREG_ZG_OFFS_USRH 0x17 64 | #define MPUREG_ZG_OFFS_USRL 0x18 65 | #define MPUREG_SMPLRT_DIV 0x19 66 | #define MPUREG_CONFIG 0x1A 67 | #define MPUREG_GYRO_CONFIG 0x1B 68 | #define MPUREG_ACCEL_CONFIG 0x1C 69 | #define MPUREG_ACCEL_CONFIG_2 0x1D 70 | #define MPUREG_LP_ACCEL_ODR 0x1E 71 | #define MPUREG_MOT_THR 0x1F 72 | #define MPUREG_FIFO_EN 0x23 73 | #define MPUREG_I2C_MST_CTRL 0x24 74 | #define MPUREG_I2C_SLV0_ADDR 0x25 75 | #define MPUREG_I2C_SLV0_REG 0x26 76 | #define MPUREG_I2C_SLV0_CTRL 0x27 77 | #define MPUREG_I2C_SLV1_ADDR 0x28 78 | #define MPUREG_I2C_SLV1_REG 0x29 79 | #define MPUREG_I2C_SLV1_CTRL 0x2A 80 | #define MPUREG_I2C_SLV2_ADDR 0x2B 81 | #define MPUREG_I2C_SLV2_REG 0x2C 82 | #define MPUREG_I2C_SLV2_CTRL 0x2D 83 | #define MPUREG_I2C_SLV3_ADDR 0x2E 84 | #define MPUREG_I2C_SLV3_REG 0x2F 85 | #define MPUREG_I2C_SLV3_CTRL 0x30 86 | #define MPUREG_I2C_SLV4_ADDR 0x31 87 | #define MPUREG_I2C_SLV4_REG 0x32 88 | #define MPUREG_I2C_SLV4_DO 0x33 89 | #define MPUREG_I2C_SLV4_CTRL 0x34 90 | #define MPUREG_I2C_SLV4_DI 0x35 91 | #define MPUREG_I2C_MST_STATUS 0x36 92 | #define MPUREG_INT_PIN_CFG 0x37 93 | #define MPUREG_INT_ENABLE 0x38 94 | #define MPUREG_ACCEL_XOUT_H 0x3B 95 | #define MPUREG_ACCEL_XOUT_L 0x3C 96 | #define MPUREG_ACCEL_YOUT_H 0x3D 97 | #define MPUREG_ACCEL_YOUT_L 0x3E 98 | #define MPUREG_ACCEL_ZOUT_H 0x3F 99 | #define MPUREG_ACCEL_ZOUT_L 0x40 100 | #define MPUREG_TEMP_OUT_H 0x41 101 | #define MPUREG_TEMP_OUT_L 0x42 102 | #define MPUREG_GYRO_XOUT_H 0x43 103 | #define MPUREG_GYRO_XOUT_L 0x44 104 | #define MPUREG_GYRO_YOUT_H 0x45 105 | #define MPUREG_GYRO_YOUT_L 0x46 106 | #define MPUREG_GYRO_ZOUT_H 0x47 107 | #define MPUREG_GYRO_ZOUT_L 0x48 108 | #define MPUREG_EXT_SENS_DATA_00 0x49 109 | #define MPUREG_EXT_SENS_DATA_01 0x4A 110 | #define MPUREG_EXT_SENS_DATA_02 0x4B 111 | #define MPUREG_EXT_SENS_DATA_03 0x4C 112 | #define MPUREG_EXT_SENS_DATA_04 0x4D 113 | #define MPUREG_EXT_SENS_DATA_05 0x4E 114 | #define MPUREG_EXT_SENS_DATA_06 0x4F 115 | #define MPUREG_EXT_SENS_DATA_07 0x50 116 | #define MPUREG_EXT_SENS_DATA_08 0x51 117 | #define MPUREG_EXT_SENS_DATA_09 0x52 118 | #define MPUREG_EXT_SENS_DATA_10 0x53 119 | #define MPUREG_EXT_SENS_DATA_11 0x54 120 | #define MPUREG_EXT_SENS_DATA_12 0x55 121 | #define MPUREG_EXT_SENS_DATA_13 0x56 122 | #define MPUREG_EXT_SENS_DATA_14 0x57 123 | #define MPUREG_EXT_SENS_DATA_15 0x58 124 | #define MPUREG_EXT_SENS_DATA_16 0x59 125 | #define MPUREG_EXT_SENS_DATA_17 0x5A 126 | #define MPUREG_EXT_SENS_DATA_18 0x5B 127 | #define MPUREG_EXT_SENS_DATA_19 0x5C 128 | #define MPUREG_EXT_SENS_DATA_20 0x5D 129 | #define MPUREG_EXT_SENS_DATA_21 0x5E 130 | #define MPUREG_EXT_SENS_DATA_22 0x5F 131 | #define MPUREG_EXT_SENS_DATA_23 0x60 132 | #define MPUREG_I2C_SLV0_DO 0x63 133 | #define MPUREG_I2C_SLV1_DO 0x64 134 | #define MPUREG_I2C_SLV2_DO 0x65 135 | #define MPUREG_I2C_SLV3_DO 0x66 136 | #define MPUREG_I2C_MST_DELAY_CTRL 0x67 137 | #define MPUREG_SIGNAL_PATH_RESET 0x68 138 | #define MPUREG_MOT_DETECT_CTRL 0x69 139 | #define MPUREG_USER_CTRL 0x6A 140 | #define MPUREG_PWR_MGMT_1 0x6B 141 | #define MPUREG_PWR_MGMT_2 0x6C 142 | #define MPUREG_BANK_SEL 0x6D 143 | #define MPUREG_MEM_START_ADDR 0x6E 144 | #define MPUREG_MEM_R_W 0x6F 145 | #define MPUREG_DMP_CFG_1 0x70 146 | #define MPUREG_DMP_CFG_2 0x71 147 | #define MPUREG_FIFO_COUNTH 0x72 148 | #define MPUREG_FIFO_COUNTL 0x73 149 | #define MPUREG_FIFO_R_W 0x74 150 | #define MPUREG_WHOAMI 0x75 151 | #define MPUREG_XA_OFFSET_H 0x77 152 | #define MPUREG_XA_OFFSET_L 0x78 153 | #define MPUREG_YA_OFFSET_H 0x7A 154 | #define MPUREG_YA_OFFSET_L 0x7B 155 | #define MPUREG_ZA_OFFSET_H 0x7D 156 | #define MPUREG_ZA_OFFSET_L 0x7E 157 | 158 | /* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */ 159 | 160 | #define AK8963_I2C_ADDR 0x0c // should return 0x18 161 | #define AK8963_Device_ID 0x48 162 | 163 | // Read-only Reg 164 | #define AK8963_WIA 0x00 165 | #define AK8963_INFO 0x01 166 | #define AK8963_ST1 0x02 167 | #define AK8963_HXL 0x03 168 | #define AK8963_HXH 0x04 169 | #define AK8963_HYL 0x05 170 | #define AK8963_HYH 0x06 171 | #define AK8963_HZL 0x07 172 | #define AK8963_HZH 0x08 173 | #define AK8963_ST2 0x09 174 | 175 | // Write/Read Reg 176 | #define AK8963_CNTL1 0x0A 177 | #define AK8963_CNTL2 0x0B 178 | #define AK8963_ASTC 0x0C 179 | #define AK8963_TS1 0x0D 180 | #define AK8963_TS2 0x0E 181 | #define AK8963_I2CDIS 0x0F 182 | 183 | // Read-only Reg ( ROM ) 184 | #define AK8963_ASAX 0x10 185 | #define AK8963_ASAY 0x11 186 | #define AK8963_ASAZ 0x12 187 | 188 | // Configuration bits MPU9250 189 | #define BIT_SLEEP 0x40 190 | #define BIT_H_RESET 0x80 191 | #define BITS_CLKSEL 0x07 192 | #define MPU_CLK_SEL_PLLGYROX 0x01 193 | #define MPU_CLK_SEL_PLLGYROZ 0x03 194 | #define MPU_EXT_SYNC_GYROX 0x02 195 | #define BITS_FS_250DPS 0x00 196 | #define BITS_FS_500DPS 0x08 197 | #define BITS_FS_1000DPS 0x10 198 | #define BITS_FS_2000DPS 0x18 199 | #define BITS_FS_2G 0x00 200 | #define BITS_FS_4G 0x08 201 | #define BITS_FS_8G 0x10 202 | #define BITS_FS_16G 0x18 203 | #define BITS_FS_MASK 0x18 204 | #define BITS_DLPF_CFG_256HZ_NOLPF2 0x00 205 | #define BITS_DLPF_CFG_188HZ 0x01 206 | #define BITS_DLPF_CFG_98HZ 0x02 207 | #define BITS_DLPF_CFG_42HZ 0x03 208 | #define BITS_DLPF_CFG_20HZ 0x04 209 | #define BITS_DLPF_CFG_10HZ 0x05 210 | #define BITS_DLPF_CFG_5HZ 0x06 211 | #define BITS_DLPF_CFG_2100HZ_NOLPF 0x07 212 | #define BITS_DLPF_CFG_MASK 0x07 213 | #define BIT_INT_ANYRD_2CLEAR 0x10 214 | #define BIT_RAW_RDY_EN 0x01 215 | #define BIT_I2C_IF_DIS 0x10 216 | 217 | #define READ_FLAG 0x80 218 | 219 | /* ---- Sensitivity --------------------------------------------------------- */ 220 | 221 | #define MPU9250A_2g ((float)0.000061035156f) // 0.000061035156 g/LSB 222 | #define MPU9250A_4g ((float)0.000122070312f) // 0.000122070312 g/LSB 223 | #define MPU9250A_8g ((float)0.000244140625f) // 0.000244140625 g/LSB 224 | #define MPU9250A_16g ((float)0.000488281250f) // 0.000488281250 g/LSB 225 | 226 | #define MPU9250G_250dps ((float)0.007633587786f) // 0.007633587786 dps/LSB 227 | #define MPU9250G_500dps ((float)0.015267175572f) // 0.015267175572 dps/LSB 228 | #define MPU9250G_1000dps ((float)0.030487804878f) // 0.030487804878 dps/LSB 229 | #define MPU9250G_2000dps ((float)0.060975609756f) // 0.060975609756 dps/LSB 230 | 231 | #define MPU9250M_4800uT ((float)0.6f) // 0.6 uT/LSB 232 | 233 | #define MPU9250T_85degC ((float)0.002995177763f) // 0.002995177763 degC/LSB 234 | 235 | #define Magnetometer_Sensitivity_Scale_Factor ((float)0.15f) 236 | -------------------------------------------------------------------------------- /drive/Navio/MPU9250.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | Written by Qiyong Mu (kylongmu@msn.com) 3 | Adapted for Raspberry Pi by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com) 4 | */ 5 | 6 | #include "MPU9250.h" 7 | 8 | #define G_SI 9.80665 9 | #define PI 3.14159 10 | 11 | //----------------------------------------------------------------------------------------------- 12 | 13 | MPU9250::MPU9250() 14 | { 15 | } 16 | 17 | /*----------------------------------------------------------------------------------------------- 18 | REGISTER READ & WRITE 19 | usage: use these methods to read and write MPU9250 registers over SPI 20 | -----------------------------------------------------------------------------------------------*/ 21 | 22 | unsigned int MPU9250::WriteReg(uint8_t WriteAddr, uint8_t WriteData) 23 | { 24 | unsigned char tx[2] = {WriteAddr, WriteData}; 25 | unsigned char rx[2] = {0}; 26 | 27 | SPIdev::transfer("/dev/spidev0.1", tx, rx, 2); 28 | 29 | return rx[1]; 30 | } 31 | 32 | //----------------------------------------------------------------------------------------------- 33 | 34 | unsigned int MPU9250::ReadReg(uint8_t ReadAddr) 35 | { 36 | return WriteReg(ReadAddr | READ_FLAG, 0x00); 37 | } 38 | 39 | //----------------------------------------------------------------------------------------------- 40 | 41 | void MPU9250::ReadRegs(uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes) 42 | { 43 | unsigned int i = 0; 44 | 45 | unsigned char tx[255] = {0}; 46 | unsigned char rx[255] = {0}; 47 | 48 | tx[0] = ReadAddr | READ_FLAG; 49 | 50 | SPIdev::transfer("/dev/spidev0.1", tx, rx, Bytes + 1); 51 | 52 | for(i=0; i X axis 245 | 1 -> Y axis 246 | 2 -> Z axis 247 | returns Factory Trim value 248 | -----------------------------------------------------------------------------------------------*/ 249 | 250 | void MPU9250::calib_acc() 251 | { 252 | uint8_t response[4]; 253 | int temp_scale; 254 | // read current acc scale 255 | temp_scale = WriteReg(MPUREG_ACCEL_CONFIG | READ_FLAG, 0x00); 256 | set_acc_scale(BITS_FS_8G); 257 | //ENABLE SELF TEST need modify 258 | //temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis); 259 | 260 | ReadRegs(MPUREG_SELF_TEST_X,response,4); 261 | calib_data[0] = ((response[0]&11100000) >> 3) | ((response[3]&00110000) >> 4); 262 | calib_data[1] = ((response[1]&11100000) >> 3) | ((response[3]&00001100) >> 2); 263 | calib_data[2] = ((response[2]&11100000) >> 3) | ((response[3]&00000011)); 264 | 265 | set_acc_scale(temp_scale); 266 | } 267 | 268 | //----------------------------------------------------------------------------------------------- 269 | 270 | void MPU9250::calib_mag() 271 | { 272 | uint8_t response[3]; 273 | float data; 274 | int i; 275 | 276 | WriteReg(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 277 | WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer 278 | WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer 279 | 280 | //WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes 281 | usleep(10000); 282 | //response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01 | READ_FLAG, 0x00); //Read I2C 283 | ReadRegs(MPUREG_EXT_SENS_DATA_00, response, 3); 284 | 285 | //response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C 286 | for(i=0; i<3; i++) { 287 | data = response[i]; 288 | magnetometer_ASA[i] = ((data - 128) / 256 + 1) * Magnetometer_Sensitivity_Scale_Factor; 289 | } 290 | } 291 | 292 | 293 | //----------------------------------------------------------------------------------------------- 294 | 295 | void MPU9250::update() 296 | { 297 | uint8_t response[21]; 298 | int16_t bit_data[3]; 299 | int i; 300 | 301 | //Send I2C command at first 302 | WriteReg(MPUREG_I2C_SLV0_ADDR, AK8963_I2C_ADDR | READ_FLAG); //Set the I2C slave addres of AK8963 and set for read. 303 | WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer 304 | WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer 305 | //must start your read from AK8963A register 0x03 and read seven bytes so that upon read of ST2 register 0x09 the AK8963A will unlatch the data registers for the next measurement. 306 | 307 | ReadRegs(MPUREG_ACCEL_XOUT_H, response, 21); 308 | 309 | //Get accelerometer value 310 | for(i=0; i<3; i++) { 311 | bit_data[i] = ((int16_t)response[i*2] << 8) | response[i*2+1]; 312 | } 313 | _ax = G_SI * bit_data[0] / acc_divider; 314 | _ay = G_SI * bit_data[1] / acc_divider; 315 | _az = G_SI * bit_data[2] / acc_divider; 316 | 317 | //Get temperature 318 | bit_data[0] = ((int16_t)response[i*2] << 8) | response[i*2+1]; 319 | temperature = ((bit_data[0] - 21) / 333.87) + 21; 320 | 321 | //Get gyroscope value 322 | for(i=4; i<7; i++) { 323 | bit_data[i-4] = ((int16_t)response[i*2] << 8) | response[i*2+1]; 324 | } 325 | _gx = (PI / 180) * bit_data[0] / gyro_divider; 326 | _gy = (PI / 180) * bit_data[1] / gyro_divider; 327 | _gz = (PI / 180) * bit_data[2] / gyro_divider; 328 | 329 | //Get Magnetometer value 330 | for(i=7; i<10; i++) { 331 | bit_data[i-7] = ((int16_t)response[i*2+1] << 8) | response[i*2]; 332 | } 333 | _mx = bit_data[0] * magnetometer_ASA[0]; 334 | _my = bit_data[1] * magnetometer_ASA[1]; 335 | _mz = bit_data[2] * magnetometer_ASA[2]; 336 | } 337 | -------------------------------------------------------------------------------- /drive/Navio/I2Cdev.cpp: -------------------------------------------------------------------------------- 1 | // i2cDev library collection - Main I2C device class 2 | // Abstracts bit and byte I2C R/W functions into a convenient class 3 | // 6/9/2012 by Jeff Rowberg 4 | // 5 | // Changelog: 6 | // 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire 7 | // - add compiler warnings when using outdated or IDE or limited i2cDev implementation 8 | // 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums) 9 | // 2011-10-03 - added automatic Arduino version detection for ease of use 10 | // 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications 11 | // 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x) 12 | // 2011-08-03 - added optional timeout parameter to read* methods to easily change from default 13 | // 2011-08-02 - added support for 16-bit registers 14 | // - fixed incorrect Doxygen comments on some methods 15 | // - added timeout value for read operations (thanks mem @ Arduino forums) 16 | // 2011-07-30 - changed read/write function structures to return success or byte counts 17 | // - made all methods static for multi-device memory savings 18 | // 2011-07-28 - initial release 19 | 20 | /* ============================================ 21 | i2cDev device library code is placed under the MIT license 22 | Copyright (c) 2012 Jeff Rowberg 23 | 24 | Permission is hereby granted, free of charge, to any person obtaining a copy 25 | of this software and associated documentation files (the "Software"), to deal 26 | in the Software without restriction, including without limitation the rights 27 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 28 | copies of the Software, and to permit persons to whom the Software is 29 | furnished to do so, subject to the following conditions: 30 | 31 | The above copyright notice and this permission notice shall be included in 32 | all copies or substantial portions of the Software. 33 | 34 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 35 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 36 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 37 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 38 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 39 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 40 | THE SOFTWARE. 41 | =============================================== 42 | */ 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include "I2Cdev.h" 56 | 57 | /** Default constructor. 58 | */ 59 | I2Cdev::I2Cdev() { 60 | } 61 | 62 | /** Read a single bit from an 8-bit device register. 63 | * @param devAddr I2C slave device address 64 | * @param regAddr Register regAddr to read from 65 | * @param bitNum Bit position to read (0-7) 66 | * @param data Container for single bit value 67 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 68 | * @return Status of read operation (true = success) 69 | */ 70 | int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) { 71 | uint8_t b; 72 | uint8_t count = readByte(devAddr, regAddr, &b, timeout); 73 | *data = b & (1 << bitNum); 74 | return count; 75 | } 76 | 77 | /** Read a single bit from a 16-bit device register. 78 | * @param devAddr I2C slave device address 79 | * @param regAddr Register regAddr to read from 80 | * @param bitNum Bit position to read (0-15) 81 | * @param data Container for single bit value 82 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 83 | * @return Status of read operation (true = success) 84 | */ 85 | int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) { 86 | uint16_t b; 87 | uint8_t count = readWord(devAddr, regAddr, &b, timeout); 88 | *data = b & (1 << bitNum); 89 | return count; 90 | } 91 | 92 | /** Read multiple bits from an 8-bit device register. 93 | * @param devAddr I2C slave device address 94 | * @param regAddr Register regAddr to read from 95 | * @param bitStart First bit position to read (0-7) 96 | * @param length Number of bits to read (not more than 8) 97 | * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) 98 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 99 | * @return Status of read operation (true = success) 100 | */ 101 | int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) { 102 | // 01101001 read byte 103 | // 76543210 bit numbers 104 | // xxx args: bitStart=4, length=3 105 | // 010 masked 106 | // -> 010 shifted 107 | uint8_t count, b; 108 | if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) { 109 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 110 | b &= mask; 111 | b >>= (bitStart - length + 1); 112 | *data = b; 113 | } 114 | return count; 115 | } 116 | 117 | /** Read multiple bits from a 16-bit device register. 118 | * @param devAddr I2C slave device address 119 | * @param regAddr Register regAddr to read from 120 | * @param bitStart First bit position to read (0-15) 121 | * @param length Number of bits to read (not more than 16) 122 | * @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05) 123 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 124 | * @return Status of read operation (1 = success, 0 = failure, -1 = timeout) 125 | */ 126 | int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) { 127 | // 1101011001101001 read byte 128 | // fedcba9876543210 bit numbers 129 | // xxx args: bitStart=12, length=3 130 | // 010 masked 131 | // -> 010 shifted 132 | uint8_t count; 133 | uint16_t w; 134 | if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) { 135 | uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1); 136 | w &= mask; 137 | w >>= (bitStart - length + 1); 138 | *data = w; 139 | } 140 | return count; 141 | } 142 | 143 | /** Read single byte from an 8-bit device register. 144 | * @param devAddr I2C slave device address 145 | * @param regAddr Register regAddr to read from 146 | * @param data Container for byte value read from device 147 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 148 | * @return Status of read operation (true = success) 149 | */ 150 | int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) { 151 | return readBytes(devAddr, regAddr, 1, data, timeout); 152 | } 153 | 154 | /** Read single word from a 16-bit device register. 155 | * @param devAddr I2C slave device address 156 | * @param regAddr Register regAddr to read from 157 | * @param data Container for word value read from device 158 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 159 | * @return Status of read operation (true = success) 160 | */ 161 | int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) { 162 | return readWords(devAddr, regAddr, 1, data, timeout); 163 | } 164 | 165 | /** Read multiple bytes from an 8-bit device register. 166 | * @param devAddr I2C slave device address 167 | * @param regAddr First register regAddr to read from 168 | * @param length Number of bytes to read 169 | * @param data Buffer to store read data in 170 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 171 | * @return Number of bytes read (-1 indicates failure) 172 | */ 173 | int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) { 174 | int8_t count = 0; 175 | int fd = open(I2CDEV, O_RDWR); 176 | 177 | if (fd < 0) { 178 | fprintf(stderr, "Failed to open device: %s\n", strerror(errno)); 179 | return(-1); 180 | } 181 | if (ioctl(fd, I2C_SLAVE, devAddr) < 0) { 182 | fprintf(stderr, "Failed to select device: %s\n", strerror(errno)); 183 | close(fd); 184 | return(-1); 185 | } 186 | if (write(fd, ®Addr, 1) != 1) { 187 | fprintf(stderr, "Failed to write reg: %s\n", strerror(errno)); 188 | close(fd); 189 | return(-1); 190 | } 191 | count = read(fd, data, length); 192 | 193 | if (count < 0) { 194 | fprintf(stderr, "Failed to read device(%d): %s\n", count, ::strerror(errno)); 195 | close(fd); 196 | return(-1); 197 | } else if (count != length) { 198 | fprintf(stderr, "Short read from device, expected %d, got %d\n", length, count); 199 | close(fd); 200 | return(-1); 201 | } 202 | close(fd); 203 | 204 | return count; 205 | } 206 | 207 | /** Read multiple bytes from an 8-bit device register without sending the register address. Required by MB85RC256(FRAM on Navio+) 208 | * @param devAddr I2C slave device address 209 | * @param regAddr First register regAddr to read from 210 | * @param length Number of bytes to read 211 | * @param data Buffer to store read data in 212 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 213 | * @return Number of bytes read (-1 indicates failure) 214 | */ 215 | int8_t I2Cdev::readBytesNoRegAddress(uint8_t devAddr, uint8_t length, uint8_t *data, uint16_t timeout) { 216 | int8_t count = 0; 217 | int fd = open(I2CDEV, O_RDWR); 218 | 219 | if (fd < 0) { 220 | fprintf(stderr, "Failed to open device: %s\n", strerror(errno)); 221 | return(-1); 222 | } 223 | if (ioctl(fd, I2C_SLAVE, devAddr) < 0) { 224 | fprintf(stderr, "Failed to select device: %s\n", strerror(errno)); 225 | close(fd); 226 | return(-1); 227 | } 228 | count = read(fd, data, length); 229 | 230 | if (count < 0) { 231 | fprintf(stderr, "Failed to read device(%d): %s\n", count, ::strerror(errno)); 232 | close(fd); 233 | return(-1); 234 | } else if (count != length) { 235 | fprintf(stderr, "Short read from device, expected %d, got %d\n", length, count); 236 | close(fd); 237 | return(-1); 238 | } 239 | close(fd); 240 | 241 | return count; 242 | } 243 | 244 | /** Read multiple words from a 16-bit device register. 245 | * @param devAddr I2C slave device address 246 | * @param regAddr First register regAddr to read from 247 | * @param length Number of words to read 248 | * @param data Buffer to store read data in 249 | * @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout) 250 | * @return Number of words read (0 indicates failure) 251 | */ 252 | int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) { 253 | int8_t count = 0; 254 | count = readBytes(devAddr,regAddr, length*2, reinterpret_cast(data)); 255 | return count/2; 256 | } 257 | 258 | /** write a single bit in an 8-bit device register. 259 | * @param devAddr I2C slave device address 260 | * @param regAddr Register regAddr to write to 261 | * @param bitNum Bit position to write (0-7) 262 | * @param value New bit value to write 263 | * @return Status of operation (true = success) 264 | */ 265 | bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) { 266 | uint8_t b; 267 | readByte(devAddr, regAddr, &b); 268 | b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum)); 269 | return writeByte(devAddr, regAddr, b); 270 | } 271 | 272 | /** write a single bit in a 16-bit device register. 273 | * @param devAddr I2C slave device address 274 | * @param regAddr Register regAddr to write to 275 | * @param bitNum Bit position to write (0-15) 276 | * @param value New bit value to write 277 | * @return Status of operation (true = success) 278 | */ 279 | bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) { 280 | uint16_t w; 281 | readWord(devAddr, regAddr, &w); 282 | w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum)); 283 | return writeWord(devAddr, regAddr, w); 284 | } 285 | 286 | /** Write multiple bits in an 8-bit device register. 287 | * @param devAddr I2C slave device address 288 | * @param regAddr Register regAddr to write to 289 | * @param bitStart First bit position to write (0-7) 290 | * @param length Number of bits to write (not more than 8) 291 | * @param data Right-aligned value to write 292 | * @return Status of operation (true = success) 293 | */ 294 | bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) { 295 | // 010 value to write 296 | // 76543210 bit numbers 297 | // xxx args: bitStart=4, length=3 298 | // 00011100 mask byte 299 | // 10101111 original value (sample) 300 | // 10100011 original & ~mask 301 | // 10101011 masked | value 302 | uint8_t b; 303 | if (readByte(devAddr, regAddr, &b) != 0) { 304 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 305 | data <<= (bitStart - length + 1); // shift data into correct position 306 | data &= mask; // zero all non-important bits in data 307 | b &= ~(mask); // zero all important bits in existing byte 308 | b |= data; // combine data with existing byte 309 | return writeByte(devAddr, regAddr, b); 310 | } else { 311 | return false; 312 | } 313 | } 314 | 315 | /** Write multiple bits in a 16-bit device register. 316 | * @param devAddr I2C slave device address 317 | * @param regAddr Register regAddr to write to 318 | * @param bitStart First bit position to write (0-15) 319 | * @param length Number of bits to write (not more than 16) 320 | * @param data Right-aligned value to write 321 | * @return Status of operation (true = success) 322 | */ 323 | bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) { 324 | // 010 value to write 325 | // fedcba9876543210 bit numbers 326 | // xxx args: bitStart=12, length=3 327 | // 0001110000000000 mask byte 328 | // 1010111110010110 original value (sample) 329 | // 1010001110010110 original & ~mask 330 | // 1010101110010110 masked | value 331 | uint16_t w; 332 | if (readWord(devAddr, regAddr, &w) != 0) { 333 | uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1); 334 | data <<= (bitStart - length + 1); // shift data into correct position 335 | data &= mask; // zero all non-important bits in data 336 | w &= ~(mask); // zero all important bits in existing word 337 | w |= data; // combine data with existing word 338 | return writeWord(devAddr, regAddr, w); 339 | } else { 340 | return false; 341 | } 342 | } 343 | 344 | /** Write single byte to an 8-bit device register. 345 | * @param devAddr I2C slave device address 346 | * @param regAddr Register address to write to 347 | * @param data New byte value to write 348 | * @return Status of operation (true = success) 349 | */ 350 | bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) { 351 | return writeBytes(devAddr, regAddr, 1, &data); 352 | } 353 | 354 | /** Write single word to a 16-bit device register. 355 | * @param devAddr I2C slave device address 356 | * @param regAddr Register address to write to 357 | * @param data New word value to write 358 | * @return Status of operation (true = success) 359 | */ 360 | bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) { 361 | return writeWords(devAddr, regAddr, 1, &data); 362 | } 363 | 364 | /** Write multiple bytes to an 8-bit device register. 365 | * @param devAddr I2C slave device address 366 | * @param regAddr First register address to write to 367 | * @param length Number of bytes to write 368 | * @param data Buffer to copy new data from 369 | * @return Status of operation (true = success) 370 | */ 371 | bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) { 372 | int8_t count = 0; 373 | uint8_t buf[128]; 374 | int fd; 375 | 376 | if (length > 127) { 377 | fprintf(stderr, "Byte write count (%d) > 127\n", length); 378 | return(FALSE); 379 | } 380 | 381 | fd = open(I2CDEV , O_RDWR); 382 | if (fd < 0) { 383 | fprintf(stderr, "Failed to open device: %s\n", strerror(errno)); 384 | return(FALSE); 385 | } 386 | if (ioctl(fd, I2C_SLAVE, devAddr) < 0) { 387 | fprintf(stderr, "Failed to select device: %s\n", strerror(errno)); 388 | close(fd); 389 | return(FALSE); 390 | } 391 | buf[0] = regAddr; 392 | memcpy(buf+1,data,length); 393 | count = write(fd, buf, length+1); 394 | if (count < 0) { 395 | fprintf(stderr, "Failed to write device(%d): %s\n", count, ::strerror(errno)); 396 | close(fd); 397 | return(FALSE); 398 | } else if (count != length+1) { 399 | fprintf(stderr, "Short write to device, expected %d, got %d\n", length+1, count); 400 | close(fd); 401 | return(FALSE); 402 | } 403 | close(fd); 404 | 405 | return TRUE; 406 | } 407 | 408 | /** Write multiple words to a 16-bit device register. 409 | * @param devAddr I2C slave device address 410 | * @param regAddr First register address to write to 411 | * @param length Number of words to write 412 | * @param data Buffer to copy new data from 413 | * @return Status of operation (true = success) 414 | */ 415 | bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) { 416 | int8_t count = 0; 417 | uint8_t buf[128]; 418 | int i, fd; 419 | 420 | // Should do potential byteswap and call writeBytes() really, but that 421 | // messes with the callers buffer 422 | 423 | if (length > 63) { 424 | fprintf(stderr, "Word write count (%d) > 63\n", length); 425 | return(FALSE); 426 | } 427 | 428 | fd = open(I2CDEV, O_RDWR); 429 | if (fd < 0) { 430 | fprintf(stderr, "Failed to open device: %s\n", strerror(errno)); 431 | return(FALSE); 432 | } 433 | if (ioctl(fd, I2C_SLAVE, devAddr) < 0) { 434 | fprintf(stderr, "Failed to select device: %s\n", strerror(errno)); 435 | close(fd); 436 | return(FALSE); 437 | } 438 | buf[0] = regAddr; 439 | for (i = 0; i < length; i++) { 440 | buf[i*2+1] = data[i] >> 8; 441 | buf[i*2+2] = data[i]; 442 | } 443 | count = write(fd, buf, length*2+1); 444 | if (count < 0) { 445 | fprintf(stderr, "Failed to write device(%d): %s\n", count, ::strerror(errno)); 446 | close(fd); 447 | return(FALSE); 448 | } else if (count != length*2+1) { 449 | fprintf(stderr, "Short write to device, expected %d, got %d\n", length+1, count); 450 | close(fd); 451 | return(FALSE); 452 | } 453 | close(fd); 454 | return TRUE; 455 | } 456 | 457 | /** Default timeout value for read operations. 458 | * Set this to 0 to disable timeout detection. 459 | */ 460 | uint16_t I2Cdev::readTimeout = 0; 461 | -------------------------------------------------------------------------------- /drive/Navio/Ublox.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | GPS driver dcode is placed under the BSD license. 3 | Written by Egor Fedorov (egor.fedorov@emlid.com) 4 | Copyright (c) 2014, Emlid Limited 5 | All rights reserved. 6 | 7 | Redistribution and use in source and binary forms, with or without 8 | modification, are permitted provided that the following conditions are met: 9 | * Redistributions of source code must retain the above copyright 10 | notice, this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright 12 | notice, this list of conditions and the following disclaimer in the 13 | documentation and/or other materials provided with the distribution. 14 | * Neither the name of the Emlid Limited nor the names of its contributors 15 | may be used to endorse or promote products derived from this software 16 | without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY 22 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 23 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 24 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 25 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 26 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 27 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include "Ublox.h" 31 | 32 | // class UBXScanner 33 | 34 | UBXScanner::UBXScanner() 35 | { 36 | reset(); 37 | } 38 | 39 | unsigned char* UBXScanner::getMessage() 40 | { 41 | return message; 42 | } 43 | 44 | unsigned int UBXScanner::getMessageLength() 45 | { 46 | return message_length; 47 | } 48 | 49 | unsigned int UBXScanner::getPosition() 50 | { 51 | return position; 52 | } 53 | 54 | void UBXScanner::reset() 55 | { 56 | message_length = 0; 57 | position = 0; 58 | state = Sync1; 59 | } 60 | 61 | int UBXScanner::update(unsigned char data) 62 | { 63 | if (state != Done) 64 | message[position++] = data; 65 | 66 | switch (state) 67 | { 68 | case Sync1: 69 | if (data == 0xb5) 70 | state = Sync2; 71 | else 72 | reset(); 73 | break; 74 | 75 | case Sync2: 76 | if (data == 0x62) 77 | state = Class; 78 | else 79 | if (data == 0xb5) 80 | state = Sync1; 81 | else 82 | reset(); 83 | break; 84 | 85 | case Class: 86 | state = ID; 87 | break; 88 | 89 | case ID: 90 | state = Length1; 91 | break; 92 | 93 | case Length1: 94 | payload_length = data; 95 | state = Length2; 96 | break; 97 | 98 | case Length2: 99 | payload_length += data << 8; 100 | state = Payload; 101 | break; 102 | 103 | case Payload: 104 | if (position == payload_length + 6) 105 | state = CK_A; 106 | if (position >= 1022) 107 | reset(); 108 | break; 109 | 110 | case CK_A: 111 | state = CK_B; 112 | break; 113 | 114 | case CK_B: 115 | message_length = 6 + payload_length + 2; 116 | state = Done; 117 | break; 118 | 119 | case Done: 120 | default: 121 | break; 122 | } 123 | 124 | return state; 125 | } 126 | 127 | // class UBXParser 128 | 129 | 130 | UBXParser::UBXParser(UBXScanner* ubxsc) : scanner(ubxsc), message(ubxsc->getMessage()) 131 | { 132 | 133 | } 134 | 135 | // This function updates message length and end position in the scanner's buffer 136 | 137 | void UBXParser::updateMessageData(){ 138 | length = scanner->getMessageLength(); 139 | position = scanner->getPosition(); 140 | } 141 | 142 | // Function decodeMessage() returns 1 in case of a successful message verification. 143 | // It looks through the buffer, checks 2 ubx protocol sync chars (0xb5 and 0x62), 144 | // counts the checksum and if everything is alright, defines the message type by id. 145 | // In this example we only decode two messages: Nav-Status and Nav-Posllh. It is possible to add more 146 | // id cases 147 | 148 | int UBXParser::decodeMessage(std::vector& data) 149 | { 150 | int flag=1; 151 | int pos; 152 | uint16_t id; 153 | uint8_t CK_A=0, CK_B=0; 154 | 155 | updateMessageData(); // get the length and end message coordinate from UBX scanner 156 | 157 | pos = position-length; // count the message start position 158 | 159 | // All UBX messages start with 2 sync chars: 0xb5 and 0x62 160 | 161 | if (*(message+pos)!=0xb5) flag=0; 162 | if (*(message+pos+1)!=0x62) flag=0; 163 | 164 | // Count the checksum 165 | 166 | for (unsigned int i=2;i<(length-2);i++){ 167 | CK_A += *(message+pos+i); 168 | CK_B += CK_A; 169 | } 170 | 171 | if (CK_A != *(message+pos+length-2)) flag=0; 172 | if (CK_B != *(message+pos+length-1)) flag=0; 173 | 174 | // If the sync chars, or the checksum is wrong, we should not be doing this anymore 175 | 176 | if (flag==0) return 0; 177 | 178 | // If we got everything right, then it's time to decide, what type of message this is 179 | 180 | id = (*(message+pos+2)) << 8 | (*(message+pos+3)); // ID is a two-byte number with little endianness 181 | 182 | flag = id; // will return message id, if we got the info decoded 183 | 184 | switch(id){ 185 | case 258: 186 | // ID for Nav-Posllh messages is 0x0102 == 258 187 | // In this example we extract 7 variables - longitude, latitude, 188 | // height above ellipsoid and mean sea level, horizontal and vertical 189 | // accuracy estimate and iTOW - GPS Millisecond Time of Week 190 | 191 | // All the needed parameters are 4-byte numbers with little endianness. 192 | // We know the current message and we want to update the info in the data vector. 193 | // First we clear the old data: 194 | 195 | data.clear(); 196 | 197 | // Second, we extract the needed data from the message buffer and save it to the vector. 198 | 199 | //iTOW 200 | data.push_back ((unsigned)((*(message+pos+9) << 24) | (*(message+pos+8) << 16) | (*(message+pos+7) << 8) | (*(message+pos+6)))); 201 | //Longitude 202 | data.push_back ((*(message+pos+13) << 24) | (*(message+pos+12) << 16) | (*(message+pos+11) << 8) | (*(message+pos+10))); 203 | //Latitude 204 | data.push_back ((*(message+pos+17) << 24) | (*(message+pos+16) << 16) | (*(message+pos+15) << 8) | (*(message+pos+14))); 205 | //Height above Ellipsoid 206 | data.push_back ((*(message+pos+21) << 24) | (*(message+pos+20) << 16) | (*(message+pos+19) << 8) | (*(message+pos+18))); 207 | //Height above mean sea level 208 | data.push_back ((*(message+pos+25) << 24) | (*(message+pos+24) << 16) | (*(message+pos+23) << 8) | (*(message+pos+22))); 209 | //Horizontal Accuracy Estateimate 210 | data.push_back ((unsigned)((*(message+pos+29) << 24) | (*(message+pos+28) << 16) | (*(message+pos+27) << 8) | (*(message+pos+26)))); 211 | //Vertical Accuracy Estateimate 212 | data.push_back ((unsigned)((*(message+pos+33) << 24) | (*(message+pos+32) << 16) | (*(message+pos+31) << 8) | (*(message+pos+30)))); 213 | break; 214 | 215 | case 259: 216 | // ID for Nav-Status messages is 0x0103 == 259 217 | // This message contains a lot of information, but the reason we use it the GPS fix status 218 | // This status is a one byte flag variable, with the offset of 10: first 6 bytes of the captured 219 | // message contain message header, id, payload length, next 4 are the first payload variable - iTOW. 220 | // We are not interested in it, so we just proceed to the fifth byte - gpsFix flag. 221 | // We are also interested in checking the gpsFixOk flag in the next byte/ 222 | data.clear(); 223 | // gpsFix 224 | data.push_back (*(message+pos+10)); 225 | // flags, which contain gpsFixOk 226 | data.push_back (*(message+pos+11)); 227 | 228 | break; 229 | 230 | default: 231 | // In case we don't want to decode the received message 232 | flag = 0; 233 | 234 | break; 235 | } 236 | 237 | return flag; 238 | } 239 | 240 | // Function checkMessage() returns 1 if the message, currently stored in the buffer is valid. 241 | // Validity means, that the necessary sync chars are present and the checksum test is passed 242 | 243 | int UBXParser::checkMessage() 244 | { 245 | int flag=1; 246 | int pos; 247 | uint8_t CK_A=0, CK_B=0; 248 | 249 | updateMessageData(); // get the length and end message coordinate from UBX scanner 250 | 251 | pos = position-length; // count the message start position 252 | 253 | // All UBX messages start with 2 sync chars: 0xb5 and 0x62 254 | 255 | if (*(message+pos)!=0xb5) flag=0; 256 | if (*(message+pos+1)!=0x62) flag=0; 257 | 258 | // Count the checksum 259 | 260 | for (unsigned int i=2;i<(length-2);i++){ 261 | CK_A += *(message+pos+i); 262 | CK_B += CK_A; 263 | } 264 | 265 | if (CK_A != *(message+pos+length-2)) flag=0; 266 | if (CK_B != *(message+pos+length-1)) flag=0; 267 | 268 | return flag; 269 | } 270 | 271 | // class Ublox 272 | 273 | Ublox::Ublox(std::string name) : spi_device_name(name), scanner(new UBXScanner()), parser(new UBXParser(scanner)) 274 | { 275 | 276 | } 277 | 278 | Ublox::Ublox(std::string name, UBXScanner* scan, UBXParser* pars) : spi_device_name(name), scanner(scan), parser(pars) 279 | { 280 | 281 | } 282 | 283 | int Ublox::enableNAV_POSLLH() 284 | { 285 | unsigned char gps_nav_posllh[] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x02, 0x01, 0x0E, 0x47}; 286 | int gps_nav_posllh_length = (sizeof(gps_nav_posllh)/sizeof(*gps_nav_posllh)); 287 | unsigned char from_gps_data_nav[gps_nav_posllh_length]; 288 | 289 | return SPIdev::transfer(spi_device_name.c_str(), gps_nav_posllh, from_gps_data_nav, gps_nav_posllh_length, 200000); 290 | } 291 | 292 | int Ublox::enableNAV_STATUS() 293 | { 294 | unsigned char gps_nav_status[] = {0xb5, 0x62, 0x06, 0x01, 0x03, 0x00, 0x01, 0x03, 0x01, 0x0F, 0x49}; 295 | int gps_nav_status_length = (sizeof(gps_nav_status)/sizeof(*gps_nav_status)); 296 | unsigned char from_gps_data_nav[gps_nav_status_length]; 297 | 298 | return SPIdev::transfer(spi_device_name.c_str(), gps_nav_status, from_gps_data_nav, gps_nav_status_length, 200000); 299 | } 300 | 301 | int Ublox::testConnection() 302 | { 303 | int status; 304 | int count = 0; 305 | unsigned char to_gps_data = 0x00, from_gps_data = 0x00; 306 | 307 | // we do this, so that at least one ubx message is enabled 308 | 309 | if (enableNAV_POSLLH()<0) 310 | { 311 | std::cerr << "Could not configure ublox over SPI\n"; 312 | } 313 | 314 | if (enableNAV_STATUS()<0) 315 | { 316 | std::cerr << "Could not configure ublox over SPI\n"; 317 | } 318 | 319 | while (count < buffer_length/2) 320 | { 321 | // From now on, we will send zeroes to the receiver, which it will ignore 322 | // However, we are simultaneously getting useful information from it 323 | SPIdev::transfer(spi_device_name.c_str(), &to_gps_data, &from_gps_data, 1, 200000); 324 | 325 | // Scanner checks the message structure with every byte received 326 | status = scanner->update(from_gps_data); 327 | 328 | if (status == UBXScanner::Done) 329 | { 330 | // Once we have a full message we decode it and reset the scanner, making it look for another message 331 | // in the data stream, coming over SPI 332 | 333 | // If we find at least one valid message in the buffer, we consider connection to be established 334 | if(parser->checkMessage()==1) 335 | { 336 | scanner->reset(); 337 | return 1; 338 | } 339 | 340 | scanner->reset(); 341 | } 342 | 343 | count++; 344 | } 345 | 346 | return 0; 347 | } 348 | 349 | int Ublox::decodeMessages() 350 | { 351 | int status; 352 | unsigned char to_gps_data = 0x00, from_gps_data = 0x00; 353 | std::vector position_data; 354 | 355 | if (enableNAV_POSLLH()<0) 356 | { 357 | std::cerr << "Could not configure ublox over SPI\n"; 358 | } 359 | 360 | if (enableNAV_STATUS()<0) 361 | { 362 | std::cerr << "Could not configure ublox over SPI\n"; 363 | } 364 | 365 | while (true) 366 | { 367 | // From now on, we will send zeroes to the receiver, which it will ignore 368 | // However, we are simultaneously getting useful information from it 369 | SPIdev::transfer(spi_device_name.c_str(), &to_gps_data, &from_gps_data, 1, 200000); 370 | 371 | // Scanner checks the message structure with every byte received 372 | status = scanner->update(from_gps_data); 373 | 374 | if (status == UBXScanner::Done) 375 | { 376 | // Once we have a full message we decode it and reset the scanner, making it look for another message 377 | // in the data stream, coming over SPI 378 | if (parser->decodeMessage(position_data) > 0) 379 | { 380 | // do something with the obtained data 381 | // 382 | // in case if NAV-POSLLH messages we can do this: 383 | // printf("decodeMessages(): \nCurrent location data:\n"); 384 | // printf("GPS Millisecond Time of Week: %lf\n", position_data[0]/1000); 385 | // printf("Longitude: %lf\n", position_data[1]/10000000); 386 | // printf("Latitude: %lf\n", position_data[2]/10000000); 387 | // printf("Height above Ellipsoid: %.3lf m\n", pos_data[3]/1000); 388 | // printf("Height above mean sea level: %.3lf m\n", pos_data[4]/1000); 389 | // printf("Horizontal Accuracy Estateimate: %.3lf m\n", pos_data[5]/1000); 390 | // printf("Vertical Accuracy Estateimate: %.3lf m\n", pos_data[6]/1000); 391 | // 392 | // in case of NAV-STATUS messages we can do this: 393 | // 394 | // printf("Current GPS status:\n"); 395 | // printf("gpsFixOk: %d\n", ((int)pos_data[1] & 0x01)); 396 | // 397 | // printf("gps Fix status: "); 398 | // switch((int)pos_data[0]){ 399 | // case 0x00: 400 | // printf("no fix\n"); 401 | // break; 402 | // 403 | // case 0x01: 404 | // printf("dead reckoning only\n"); 405 | // break; 406 | // 407 | // case 0x02: 408 | // printf("2D-fix\n"); 409 | // break; 410 | // 411 | // case 0x03: 412 | // printf("3D-fix\n"); 413 | // break; 414 | // 415 | // case 0x04: 416 | // printf("GPS + dead reckoning combined\n"); 417 | // break; 418 | // 419 | // case 0x05: 420 | // printf("Time only fix\n"); 421 | // break; 422 | // 423 | // default: 424 | // printf("Reserved value. Current state unknown\n"); 425 | // break; 426 | // 427 | // } 428 | // 429 | // printf("\n"); 430 | 431 | } 432 | 433 | scanner->reset(); 434 | } 435 | 436 | usleep(200); 437 | 438 | } 439 | 440 | return 0 ; 441 | } 442 | 443 | int Ublox::decodeSingleMessage(message_t msg, std::vector& position_data) 444 | { 445 | switch(msg){ 446 | case NAV_POSLLH: 447 | { 448 | uint16_t id = 0x0102; 449 | int status; 450 | int count = 0; 451 | unsigned char to_gps_data = 0x00, from_gps_data = 0x00; 452 | 453 | while (count < buffer_length/2) 454 | { 455 | // From now on, we will send zeroes to the receiver, which it will ignore 456 | // However, we are simultaneously getting useful information from it 457 | SPIdev::transfer(spi_device_name.c_str(), &to_gps_data, &from_gps_data, 1, 200000); 458 | 459 | // Scanner checks the message structure with every byte received 460 | status = scanner->update(from_gps_data); 461 | 462 | if (status == UBXScanner::Done) 463 | { 464 | // Once we have a full message we decode it and reset the scanner, making it look for another message 465 | // in the data stream, coming over SPI 466 | if(parser->decodeMessage(position_data) == id) 467 | { 468 | // Now let's do something with the extracted information 469 | // in case of NAV-POSLLH messages we can print the information like this: 470 | // printf("GPS Millisecond Time of Week: %lf\n", position_data[0]/1000); 471 | // printf("Longitude: %lf\n", position_data[1]/10000000); 472 | // printf("Latitude: %lf\n", position_data[2]/10000000); 473 | // printf("Height above Ellipsoid: %.3lf m\n", pos_data[3]/1000); 474 | // printf("Height above mean sea level: %.3lf m\n", pos_data[4]/1000); 475 | // printf("Horizontal Accuracy Estateimate: %.3lf m\n", pos_data[5]/1000); 476 | // printf("Vertical Accuracy Estateimate: %.3lf m\n", pos_data[6]/1000); 477 | 478 | 479 | // You can see ubx message structure in ublox reference manual 480 | 481 | scanner->reset(); 482 | 483 | return 1; 484 | } 485 | 486 | scanner->reset(); 487 | } 488 | 489 | count++; 490 | } 491 | 492 | return 0; 493 | } 494 | 495 | break; 496 | 497 | case NAV_STATUS: 498 | { 499 | uint16_t id = 0x0103; 500 | int status; 501 | int count = 0; 502 | unsigned char to_gps_data = 0x00, from_gps_data = 0x00; 503 | 504 | while (count < buffer_length/2) 505 | { 506 | // From now on, we will send zeroes to the receiver, which it will ignore 507 | // However, we are simultaneously getting useful information from it 508 | SPIdev::transfer(spi_device_name.c_str(), &to_gps_data, &from_gps_data, 1, 200000); 509 | 510 | // Scanner checks the message structure with every byte received 511 | status = scanner->update(from_gps_data); 512 | 513 | if (status == UBXScanner::Done) 514 | { 515 | // Once we have a full message we decode it and reset the scanner, making it look for another message 516 | // in the data stream, coming over SPI 517 | if(parser->decodeMessage(position_data) == id) 518 | { 519 | // Now let's do something with the extracted information 520 | // in case of NAV-STATUS messages we can do this: 521 | // 522 | // printf("Current GPS status:\n"); 523 | // printf("gpsFixOk: %d\n", ((int)pos_data[1] & 0x01)); 524 | // 525 | // printf("gps Fix status: "); 526 | // switch((int)pos_data[0]){ 527 | // case 0x00: 528 | // printf("no fix\n"); 529 | // break; 530 | // 531 | // case 0x01: 532 | // printf("dead reckoning only\n"); 533 | // break; 534 | // 535 | // case 0x02: 536 | // printf("2D-fix\n"); 537 | // break; 538 | // 539 | // case 0x03: 540 | // printf("3D-fix\n"); 541 | // break; 542 | // 543 | // case 0x04: 544 | // printf("GPS + dead reckoning combined\n"); 545 | // break; 546 | // 547 | // case 0x05: 548 | // printf("Time only fix\n"); 549 | // break; 550 | // 551 | // default: 552 | // printf("Reserved value. Current state unknown\n"); 553 | // break; 554 | // 555 | // } 556 | // 557 | // printf("\n"); 558 | 559 | // You can see ubx message structure in ublox reference manual 560 | 561 | scanner->reset(); 562 | 563 | return 1; 564 | } 565 | 566 | scanner->reset(); 567 | } 568 | 569 | count++; 570 | } 571 | 572 | return 0; 573 | } 574 | 575 | break; 576 | 577 | 578 | // add your ubx message type here! 579 | 580 | default: 581 | return 0; 582 | 583 | break; 584 | } 585 | } 586 | --------------------------------------------------------------------------------