h3n3/darkwater_cplus_escape
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Initial escape library

Browse Source 
Not working yet
This commit is contained in:
shrkey
2016-10-10 22:42:09 +01:00
parent d3429c677a
commit 9f4dffe704
24 changed files with 2872 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

349
darkwater/DWESCAPE.cpp Normal file
View File

@@ -0,0 +1,349 @@
/*
Dark Water 640 driver code is placed under the BSD license.
Written by Team Dark Water (team@darkwater.io)
Copyright (c) 2014, Dark Water
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Emlid Limited nor the names of its contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "DWESCAPE.h"
/** PCA9685 constructor.
* @param address I2C address
* @see DWESCAPE_DEFAULT_ADDRESS
*/
DWESCAPE::DWESCAPE(uint8_t address) {
this->devAddr = address;
}
/** Power on and prepare for general usage.
* This method reads prescale value stored in PCA9685 and calculate frequency based on it.
* Then it enables auto-increment of register address to allow for faster writes.
* And finally the restart is performed to enable clocking.
*/
bool DWESCAPE::initialize() {
this->pwm = new PCA9685( this->devAddr );
this->pwm->initialize();
if (!testConnection() ) {
printf("No 640 board found\n");
return 0;
}
// set the default frequency
setFrequency( 100 );
}
/** Verify the I2C connection.
* @return True if connection is valid, false otherwise
*/
bool DWESCAPE::testConnection() {
return this->pwm->testConnection();
}
/** Calculate prescale value based on the specified frequency and write it to the device.
* @return Frequency in Hz
*/
float DWESCAPE::getFrequency() {
return this->pwm->getFrequency();
}
/** Calculate prescale value based on the specified frequency and write it to the device.
* @param Frequency in Hz
*/
void DWESCAPE::setFrequency(float frequency) {
this->pwm->setFrequency( frequency );
}
/** Set channel start offset of the pulse and it's length
* @param Channel number (0-15)
* @param Offset (0-4095)
* @param Length (0-4095)
*/
void DWESCAPE::setPWM(uint8_t channel, uint16_t offset, uint16_t length) {
this->pwm->setPWM( channel, offset, length );
}
/** Set channel's pulse length
* @param Channel number (0-15)
* @param Length (0-4095)
*/
void DWESCAPE::setPWM(uint8_t channel, uint16_t length) {
this->pwm->setPWM(channel, length);
}
/** Set channel's pulse length in milliseconds
* @param Channel number (0-15)
* @param Length in milliseconds
*/
void DWESCAPE::setPWMmS(uint8_t channel, float length_mS) {
this->pwm->setPWMmS(channel, length_mS);
}
/** Set channel's pulse length in microseconds
* @param Channel number (0-15)
* @param Length in microseconds
*/
void DWESCAPE::setPWMuS(uint8_t channel, float length_uS) {
this->pwm->setPWMuS(channel, length_uS);
}
/** Set start offset of the pulse and it's length for all channels
* @param Offset (0-4095)
* @param Length (0-4095)
*/
void DWESCAPE::setAllPWM(uint16_t offset, uint16_t length) {
this->pwm->setAllPWM(offset, length);
}
/** Set pulse length for all channels
* @param Length (0-4095)
*/
void DWESCAPE::setAllPWM(uint16_t length) {
this->pwm->setAllPWM(length);
}
/** Set pulse length in milliseconds for all channels
* @param Length in milliseconds
*/
void DWESCAPE::setAllPWMmS(float length_mS) {
this->pwm->setAllPWMmS(length_mS);
}
/** Set pulse length in microseconds for all channels
* @param Length in microseconds
*/
void DWESCAPE::setAllPWMuS(float length_uS) {
this->pwm->setAllPWMuS(length_uS);
}
void DWESCAPE::setPin(uint8_t channel, uint8_t value) {
if( channel < 0 || channel > 15 ) {
fprintf(stderr, "PWM pin must be between 0 and 15 inclusive");
}
if( value == 0 ) {
setPWM( channel, 0, 4096 );
} else if( value == 1 ) {
setPWM( channel, 4096, 0 );
} else {
fprintf(stderr, "Pin value must be 0 or 1!");
}
}
void DWESCAPE::setAllPin(uint8_t value) {
if( value == 0 ) {
setAllPWM( 0, 4096 );
} else if( value == 1 ) {
setAllPWM( 4096, 0 );
} else {
fprintf(stderr, "Pin value must be 0 or 1!");
}
}
void DWESCAPE::allOff() {
setAllPin( 0 );
}
/* DC Motor specific code */
DW_Motor *DWESCAPE::getMotor(uint8_t motor) {
uint8_t pin;
motor--;
// Get the motor
switch(motor) {
case 0:
pin = 0;
break;
case 1:
pin = 1;
break;
case 2:
pin = 2;
break;
case 3:
pin = 3;
break;
case 4:
pin = 4;
break;
case 5:
pin = 5;
break;
default:
fprintf(stderr, "Motor number must be between 1 and 6 inclusive");
}
if(motors[motor].motor == 0) {
// We don't have one yet
motors[motor].motor = motor; // how many motors on one line?!?
motors[motor].DWC = this;
motors[motor].pin = pin;
}
return &motors[motor];
}
DW_Servo *DWESCAPE::getServo(uint8_t servo) {
uint8_t pin;
servo--;
// Get the servo
switch(servo) {
case 0:
pin = 6; // Servo 1 is on PWM 0
break;
case 1:
pin = 7; // Servo 2 is on PWM 1
break;
case 2:
pin = 8;
break;
case 3:
pin = 9;
break;
case 4:
pin = 10;
break;
case 5:
pin = 11;
break;
default:
fprintf(stderr, "Servo number must be between 1 and 6 inclusive");
}
if(servos[servo].servo == 0) {
// We don't have one yet
servos[servo].servo = servo; // how many servos on one line?!?
servos[servo].DWC = this;
servos[servo].pin = pin;
}
return &servos[servo];
}
/* Motors code */
DW_Motor::DW_Motor(void) {
DWC = NULL;
motor = 0;
pin = 0;
}
void DW_Motor::setMotorSpeed(int16_t speed) {
// Speed deciphering for the two control modes
if( speed >= 1000 && speed < 1500 ) {
run( DW_REVERSE, map(speed, 1500, 1000, 0, 255 ) );
} else if( speed > 1500 && speed <= 2000 ) {
run( DW_FORWARD, map(speed, 1500, 2000, 0, 255 ) );
} else if( speed > 0 && speed <= 255 ) {
run( DW_FORWARD, speed );
} else if( speed < 0 && speed >= -255 ) {
run( DW_REVERSE, abs(speed) );
} else if( speed == 0 || speed == 1500 ) {
run( DW_STOP, speed );
}
}
void DW_Motor::off(void) {
setMotorSpeed( 0 );
}
void DW_Motor::run( uint8_t control, uint16_t speed ) {
// get the mode
if( DWC->getMode() == DW_PHASE ) {
if( control == DW_FORWARD ) {
DWC->setPin( in2, 0 );
DWC->setPWM( in1, 0, speed * 16 );
} else if( control == DW_REVERSE ) {
DWC->setPin( in2, 1 );
DWC->setPWM( in1, 0, speed * 16 );
} else if( control == DW_STOP ) {
DWC->setPin( in2, 0 );
DWC->setPin( in1, 0 );
}
} else { // DW_ININ
if( control == DW_FORWARD ) {
DWC->setPin( in2, 0 );
DWC->setPWM( in1, 0, speed * 16 );
} else if( control == DW_REVERSE ) {
DWC->setPin( in1, 0 );
DWC->setPWM( in2, 0, speed * 16 );
} else if( control == DW_STOP ) {
DWC->setPin( in1, 1 );
DWC->setPin( in2, 1 );
} else if( control == DW_COAST ) {
DWC->setPin( in1, 0 );
DWC->setPin( in2, 0 );
}
}
}
uint16_t DW_Motor::map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max)
{
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
/* Servo code */
DW_Servo::DW_Servo(void) {
DWC = NULL;
servo = 0;
pin = 0;
}
void DW_Servo::off(void) {
DWC->setPin( pin, 0 );
}
void DW_Servo::setPWMmS(float length_mS) {
DWC->setPWMmS( pin, length_mS ); // Servo 1 is on PWM 0
}
void DW_Servo::setPWMuS(float length_uS) {
DWC->setPWMuS( pin, length_uS ); // Servo 1 is on PWM 0
}

135
darkwater/DWESCAPE.h Normal file
View File

@@ -0,0 +1,135 @@
/*
Dark Water ESCAPE driver code is placed under the BSD license.
Written by Team Dark Water (team@darkwater.io)
Copyright (c) 2014, Dark Water
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Emlid Limited nor the names of its contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef DWESCAPE_H
#define DWESCAPE_H
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>
#include <string>
#include "PCA9685.h"
#include "gpio.h"
#define DWESCAPE_DEFAULT_ADDRESS 0x61 // ESCAPE default
#define DW_FORWARD 1
#define DW_REVERSE 2
#define DW_BRAKEFORWARD 3
#define DW_BRAKEREVERSE 4
#define DW_STOP 5
#define DW_COAST 6
using namespace DarkWater;
class DWESCAPE;
class DW_Motor {
public:
DW_Motor(void);
friend class DWESCAPE;
void setMotorSpeed(int16_t speed);
void off(void);
void run(uint8_t control, uint16_t speed );
private:
uint8_t pin;
DWESCAPE *DWC;
uint8_t motor;
uint16_t map(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max);
};
class DW_Servo {
public:
DW_Servo(void);
friend class DWESCAPE;
void off(void);
void setPWMmS(float length_mS);
void setPWMuS(float length_uS);
private:
uint8_t pin;
DWESCAPE *DWC;
uint8_t servo;
};
class DWESCAPE {
public:
DWESCAPE(uint8_t address = DWESCAPE_DEFAULT_ADDRESS);
bool initialize();
bool testConnection();
float getFrequency();
void setFrequency(float frequency);
void setPWM(uint8_t channel, uint16_t offset, uint16_t length);
void setPWM(uint8_t channel, uint16_t length);
void setPWMmS(uint8_t channel, float length_mS);
void setPWMuS(uint8_t channel, float length_uS);
void setAllPWM(uint16_t offset, uint16_t length);
void setAllPWM(uint16_t length);
void setAllPWMmS(float length_mS);
void setAllPWMuS(float length_uS);
void setPin(uint8_t channel, uint8_t value);
void setAllPin(uint8_t value);
void allOff();
DW_Motor *getMotor(uint8_t motor);
DW_Servo *getServo(uint8_t servo);
// void setStepperOff(uint8_t stepper);
// void setStepperSpeed(uint8_t stepper, uint16_t speed);
// void oneStep(uint8_t stepper, uint8_t direction, uint8_t style);
// void step(uint8_t stepper, uint16_t steps, uint8_t direction, uint8_t style = DW_SINGLE);
private:
uint8_t devAddr;
float frequency;
uint8_t mode;
PCA9685* pwm;
DW_Motor motors[6];
DW_Servo servos[6];
};
#endif // DWESCAPE_H

460
darkwater/I2Cdev.cpp Normal file
View File

@@ -0,0 +1,460 @@
// i2cDev library collection - Main I2C device class
// Abstracts bit and byte I2C R/W functions into a convenient class
// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
// - add compiler warnings when using outdated or IDE or limited i2cDev implementation
// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
// 2011-10-03 - added automatic Arduino version detection for ease of use
// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
// 2011-08-02 - added support for 16-bit registers
// - fixed incorrect Doxygen comments on some methods
// - added timeout value for read operations (thanks mem @ Arduino forums)
// 2011-07-30 - changed read/write function structures to return success or byte counts
// - made all methods static for multi-device memory savings
// 2011-07-28 - initial release
/* ============================================
i2cDev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
#include <stdio.h>
#include <stdint.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <linux/i2c-dev.h>
#include "I2Cdev.h"
/** Default constructor.
*/
I2Cdev::I2Cdev() {
}
/** Read a single bit from an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param bitNum Bit position to read (0-7)
* @param data Container for single bit value
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout) {
uint8_t b;
uint8_t count = readByte(devAddr, regAddr, &b, timeout);
*data = b & (1 << bitNum);
return count;
}
/** Read a single bit from a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param bitNum Bit position to read (0-15)
* @param data Container for single bit value
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout) {
uint16_t b;
uint8_t count = readWord(devAddr, regAddr, &b, timeout);
*data = b & (1 << bitNum);
return count;
}
/** Read multiple bits from an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param bitStart First bit position to read (0-7)
* @param length Number of bits to read (not more than 8)
* @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout) {
// 01101001 read byte
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 010 masked
// -> 010 shifted
uint8_t count, b;
if ((count = readByte(devAddr, regAddr, &b, timeout)) != 0) {
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
b &= mask;
b >>= (bitStart - length + 1);
*data = b;
}
return count;
}
/** Read multiple bits from a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param bitStart First bit position to read (0-15)
* @param length Number of bits to read (not more than 16)
* @param data Container for right-aligned value (i.e. '101' read from any bitStart position will equal 0x05)
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (1 = success, 0 = failure, -1 = timeout)
*/
int8_t I2Cdev::readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout) {
// 1101011001101001 read byte
// fedcba9876543210 bit numbers
// xxx args: bitStart=12, length=3
// 010 masked
// -> 010 shifted
uint8_t count;
uint16_t w;
if ((count = readWord(devAddr, regAddr, &w, timeout)) != 0) {
uint16_t mask = ((1 << length) - 1) << (bitStart - length + 1);
w &= mask;
w >>= (bitStart - length + 1);
*data = w;
}
return count;
}
/** Read single byte from an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param data Container for byte value read from device
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout) {
return readBytes(devAddr, regAddr, 1, data, timeout);
}
/** Read single word from a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to read from
* @param data Container for word value read from device
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Status of read operation (true = success)
*/
int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout) {
return readWords(devAddr, regAddr, 1, data, timeout);
}
/** Read multiple bytes from an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr First register regAddr to read from
* @param length Number of bytes to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of bytes read (-1 indicates failure)
*/
int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
int8_t count = 0;
int fd = open(I2CDEV, O_RDWR);
if (fd < 0) {
fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
return(-1);
}
if (ioctl(fd, I2C_SLAVE, devAddr) < 0) {
fprintf(stderr, "Failed to select device: %s\n", strerror(errno));
close(fd);
return(-1);
}
if (write(fd, &regAddr, 1) != 1) {
fprintf(stderr, "Failed to write reg: %s\n", strerror(errno));
close(fd);
return(-1);
}
count = read(fd, data, length);
if (count < 0) {
fprintf(stderr, "Failed to read device(%d): %s\n", count, ::strerror(errno));
close(fd);
return(-1);
} else if (count != length) {
fprintf(stderr, "Short read from device, expected %d, got %d\n", length, count);
close(fd);
return(-1);
}
close(fd);
return count;
}
/** Read multiple bytes from an 8-bit device register without sending the register address. Required by MB85RC256(FRAM on Navio+)
* @param devAddr I2C slave device address
* @param regAddr First register regAddr to read from
* @param length Number of bytes to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of bytes read (-1 indicates failure)
*/
int8_t I2Cdev::readBytesNoRegAddress(uint8_t devAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
int8_t count = 0;
int fd = open(I2CDEV, O_RDWR);
if (fd < 0) {
fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
return(-1);
}
if (ioctl(fd, I2C_SLAVE, devAddr) < 0) {
fprintf(stderr, "Failed to select device: %s\n", strerror(errno));
close(fd);
return(-1);
}
count = read(fd, data, length);
if (count < 0) {
fprintf(stderr, "Failed to read device(%d): %s\n", count, ::strerror(errno));
close(fd);
return(-1);
} else if (count != length) {
fprintf(stderr, "Short read from device, expected %d, got %d\n", length, count);
close(fd);
return(-1);
}
close(fd);
return count;
}
/** Read multiple words from a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr First register regAddr to read from
* @param length Number of words to read
* @param data Buffer to store read data in
* @param timeout Optional read timeout in milliseconds (0 to disable, leave off to use default class value in I2Cdev::readTimeout)
* @return Number of words read (0 indicates failure)
*/
int8_t I2Cdev::readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout) {
int8_t count = 0;
count = readBytes(devAddr,regAddr, length*2, reinterpret_cast<uint8_t*>(data));
return count/2;
}
/** write a single bit in an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to write to
* @param bitNum Bit position to write (0-7)
* @param value New bit value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
uint8_t b;
readByte(devAddr, regAddr, &b);
b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
return writeByte(devAddr, regAddr, b);
}
/** write a single bit in a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to write to
* @param bitNum Bit position to write (0-15)
* @param value New bit value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data) {
uint16_t w;
readWord(devAddr, regAddr, &w);
w = (data != 0) ? (w | (1 << bitNum)) : (w & ~(1 << bitNum));
return writeWord(devAddr, regAddr, w);
}
/** Write multiple bits in an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to write to
* @param bitStart First bit position to write (0-7)
* @param length Number of bits to write (not more than 8)
* @param data Right-aligned value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
// 010 value to write
// 76543210 bit numbers
// xxx args: bitStart=4, length=3
// 00011100 mask byte
// 10101111 original value (sample)
// 10100011 original & ~mask
// 10101011 masked | value
uint8_t b;
if (readByte(devAddr, regAddr, &b) != 0) {
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
b &= ~(mask); // zero all important bits in existing byte
b |= data; // combine data with existing byte
return writeByte(devAddr, regAddr, b);
} else {
return false;
}
}
/** Write multiple bits in a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register regAddr to write to
* @param bitStart First bit position to write (0-15)
* @param length Number of bits to write (not more than 16)
* @param data Right-aligned value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data) {
// 010 value to write
// fedcba9876543210 bit numbers
// xxx args: bitStart=12, length=3
// 0001110000000000 mask byte
// 1010111110010110 original value (sample)
// 1010001110010110 original & ~mask
// 1010101110010110 masked | value
uint16_t w;
if (readWord(devAddr, regAddr, &w) != 0) {
uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
data <<= (bitStart - length + 1); // shift data into correct position
data &= mask; // zero all non-important bits in data
w &= ~(mask); // zero all important bits in existing word
w |= data; // combine data with existing word
return writeWord(devAddr, regAddr, w);
} else {
return false;
}
}
/** Write single byte to an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register address to write to
* @param data New byte value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
return writeBytes(devAddr, regAddr, 1, &data);
}
/** Write single word to a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr Register address to write to
* @param data New word value to write
* @return Status of operation (true = success)
*/
bool I2Cdev::writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data) {
return writeWords(devAddr, regAddr, 1, &data);
}
/** Write multiple bytes to an 8-bit device register.
* @param devAddr I2C slave device address
* @param regAddr First register address to write to
* @param length Number of bytes to write
* @param data Buffer to copy new data from
* @return Status of operation (true = success)
*/
bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
int8_t count = 0;
uint8_t buf[128];
int fd;
if (length > 127) {
fprintf(stderr, "Byte write count (%d) > 127\n", length);
return(FALSE);
}
fd = open(I2CDEV , O_RDWR);
if (fd < 0) {
fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
return(FALSE);
}
if (ioctl(fd, I2C_SLAVE, devAddr) < 0) {
fprintf(stderr, "Failed to select device: %s\n", strerror(errno));
close(fd);
return(FALSE);
}
buf[0] = regAddr;
memcpy(buf+1,data,length);
count = write(fd, buf, length+1);
if (count < 0) {
fprintf(stderr, "Failed to write device(%d): %s\n", count, ::strerror(errno));
close(fd);
return(FALSE);
} else if (count != length+1) {
fprintf(stderr, "Short write to device, expected %d, got %d\n", length+1, count);
close(fd);
return(FALSE);
}
close(fd);
return TRUE;
}
/** Write multiple words to a 16-bit device register.
* @param devAddr I2C slave device address
* @param regAddr First register address to write to
* @param length Number of words to write
* @param data Buffer to copy new data from
* @return Status of operation (true = success)
*/
bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data) {
int8_t count = 0;
uint8_t buf[128];
int i, fd;
// Should do potential byteswap and call writeBytes() really, but that
// messes with the callers buffer
if (length > 63) {
fprintf(stderr, "Word write count (%d) > 63\n", length);
return(FALSE);
}
fd = open(I2CDEV, O_RDWR);
if (fd < 0) {
fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
return(FALSE);
}
if (ioctl(fd, I2C_SLAVE, devAddr) < 0) {
fprintf(stderr, "Failed to select device: %s\n", strerror(errno));
close(fd);
return(FALSE);
}
buf[0] = regAddr;
for (i = 0; i < length; i++) {
buf[i*2+1] = data[i] >> 8;
buf[i*2+2] = data[i];
}
count = write(fd, buf, length*2+1);
if (count < 0) {
fprintf(stderr, "Failed to write device(%d): %s\n", count, ::strerror(errno));
close(fd);
return(FALSE);
} else if (count != length*2+1) {
fprintf(stderr, "Short write to device, expected %d, got %d\n", length+1, count);
close(fd);
return(FALSE);
}
close(fd);
return TRUE;
}
/** Default timeout value for read operations.
* Set this to 0 to disable timeout detection.
*/
uint16_t I2Cdev::readTimeout = 0;

85
darkwater/I2Cdev.h Normal file
View File

@@ -0,0 +1,85 @@
// i2cDev library collection - Main I2C device class header file
// Abstracts bit and byte I2C R/W functions into a convenient class
// 6/9/2012 by Jeff Rowberg <jeff@rowberg.net>
//
// Changelog:
// 2012-06-09 - fix major issue with reading > 32 bytes at a time with Arduino Wire
// - add compiler warnings when using outdated or IDE or limited i2cDev implementation
// 2011-11-01 - fix write*Bits mask calculation (thanks sasquatch @ Arduino forums)
// 2011-10-03 - added automatic Arduino version detection for ease of use
// 2011-10-02 - added Gene Knight's NBWire TwoWire class implementation with small modifications
// 2011-08-31 - added support for Arduino 1.0 Wire library (methods are different from 0.x)
// 2011-08-03 - added optional timeout parameter to read* methods to easily change from default
// 2011-08-02 - added support for 16-bit registers
// - fixed incorrect Doxygen comments on some methods
// - added timeout value for read operations (thanks mem @ Arduino forums)
// 2011-07-30 - changed read/write function structures to return success or byte counts
// - made all methods static for multi-device memory savings
// 2011-07-28 - initial release
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
#ifndef _I2Cdev_H_
#define _I2Cdev_H_
#define RASPBERRY_PI_I2C "/dev/i2c-1"
#define BANANA_PI_I2C "/dev/i2c-2"
#define I2CDEV RASPBERRY_PI_I2C
#ifndef TRUE
#define TRUE (1==1)
#define FALSE (0==1)
#endif
#include <stdint.h>
class I2Cdev {
public:
I2Cdev();
static int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readBytesNoRegAddress(uint8_t devAddr, uint8_t length, uint8_t *data, uint16_t timeout=I2Cdev::readTimeout);
static int8_t readWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data, uint16_t timeout=I2Cdev::readTimeout);
static bool writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data);
static bool writeBitW(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint16_t data);
static bool writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data);
static bool writeBitsW(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint16_t data);
static bool writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data);
static bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data);
static bool writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data);
static bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t *data);
static uint16_t readTimeout;
};
#endif /* _I2Cdev_H_ */

474
darkwater/MPU9250.cpp Normal file
View File

@@ -0,0 +1,474 @@
/*
Written by Qiyong Mu (kylongmu@msn.com)
Adapted for Raspberry Pi by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com)
*/
#include "MPU9250.h"
#define G_SI 9.80665
#define PI 3.14159
//-----------------------------------------------------------------------------------------------
MPU9250::MPU9250()
{
}
/*-----------------------------------------------------------------------------------------------
REGISTER READ & WRITE
usage: use these methods to read and write MPU9250 registers over SPI
-----------------------------------------------------------------------------------------------*/
unsigned int MPU9250::WriteReg( uint8_t WriteAddr, uint8_t WriteData )
{
unsigned int temp_val;
unsigned char tx[2] = {WriteAddr, WriteData};
unsigned char rx[2] = {0};
SPIdev::transfer("/dev/spidev0.1", tx, rx, 2);
return rx[1];
}
//-----------------------------------------------------------------------------------------------
unsigned int MPU9250::ReadReg( uint8_t WriteAddr, uint8_t WriteData )
{
return WriteReg(WriteAddr | READ_FLAG, WriteData);
}
//-----------------------------------------------------------------------------------------------
void MPU9250::ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes )
{
unsigned int i = 0;
unsigned char tx[255] = {0};
unsigned char rx[255] = {0};
tx[0] = ReadAddr | READ_FLAG;
SPIdev::transfer("/dev/spidev0.1", tx, rx, Bytes + 1);
for(i=0; i<Bytes; i++)
ReadBuf[i] = rx[i + 1];
usleep(50);
}
/*-----------------------------------------------------------------------------------------------
TEST CONNECTION
usage: call this function to know if SPI and MPU9250 are working correctly.
returns true if mpu9250 answers
-----------------------------------------------------------------------------------------------*/
bool MPU9250::testConnection()
{
unsigned int response;
response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
if (response == 0x71)
return true;
else
return false;
}
/*-----------------------------------------------------------------------------------------------
INITIALIZATION
usage: call this function at startup, giving the sample rate divider (raging from 0 to 255) and
low pass filter value; suitable values are:
BITS_DLPF_CFG_256HZ_NOLPF2
BITS_DLPF_CFG_188HZ
BITS_DLPF_CFG_98HZ
BITS_DLPF_CFG_42HZ
BITS_DLPF_CFG_20HZ
BITS_DLPF_CFG_10HZ
BITS_DLPF_CFG_5HZ
BITS_DLPF_CFG_2100HZ_NOLPF
returns 1 if an error occurred
-----------------------------------------------------------------------------------------------*/
#define MPU_InitRegNum 16
bool MPU9250::initialize(int sample_rate_div, int low_pass_filter)
{
uint8_t i = 0;
uint8_t MPU_Init_Data[MPU_InitRegNum][2] = {
//{0x80, MPUREG_PWR_MGMT_1}, // Reset Device - Disabled because it seems to corrupt initialisation of AK8963
{0x01, MPUREG_PWR_MGMT_1}, // Clock Source
{0x00, MPUREG_PWR_MGMT_2}, // Enable Acc & Gyro
{low_pass_filter, MPUREG_CONFIG}, // Use DLPF set Gyroscope bandwidth 184Hz, temperature bandwidth 188Hz
{0x18, MPUREG_GYRO_CONFIG}, // +-2000dps
{0x08, MPUREG_ACCEL_CONFIG}, // +-4G
{0x09, MPUREG_ACCEL_CONFIG_2}, // Set Acc Data Rates, Enable Acc LPF , Bandwidth 184Hz
{0x30, MPUREG_INT_PIN_CFG}, //
//{0x40, MPUREG_I2C_MST_CTRL}, // I2C Speed 348 kHz
//{0x20, MPUREG_USER_CTRL}, // Enable AUX
{0x20, MPUREG_USER_CTRL}, // I2C Master mode
{0x0D, MPUREG_I2C_MST_CTRL}, // I2C configuration multi-master IIC 400KHz
{AK8963_I2C_ADDR, MPUREG_I2C_SLV0_ADDR}, //Set the I2C slave addres of AK8963 and set for write.
//{0x09, MPUREG_I2C_SLV4_CTRL},
//{0x81, MPUREG_I2C_MST_DELAY_CTRL}, //Enable I2C delay
{AK8963_CNTL2, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
{0x01, MPUREG_I2C_SLV0_DO}, // Reset AK8963
{0x81, MPUREG_I2C_SLV0_CTRL}, //Enable I2C and set 1 byte
{AK8963_CNTL1, MPUREG_I2C_SLV0_REG}, //I2C slave 0 register address from where to begin data transfer
{0x12, MPUREG_I2C_SLV0_DO}, // Register value to continuous measurement in 16bit
{0x81, MPUREG_I2C_SLV0_CTRL} //Enable I2C and set 1 byte
};
//spi.format(8,0);
//spi.frequency(1000000);
for(i=0; i<MPU_InitRegNum; i++) {
WriteReg(MPU_Init_Data[i][1], MPU_Init_Data[i][0]);
usleep(100000); //I2C must slow down the write speed, otherwise it won't work
}
set_acc_scale(BITS_FS_16G);
set_gyro_scale(BITS_FS_2000DPS);
calib_mag();
return 0;
}
/*-----------------------------------------------------------------------------------------------
ACCELEROMETER SCALE
usage: call this function at startup, after initialization, to set the right range for the
accelerometers. Suitable ranges are:
BITS_FS_2G
BITS_FS_4G
BITS_FS_8G
BITS_FS_16G
returns the range set (2,4,8 or 16)
-----------------------------------------------------------------------------------------------*/
unsigned int MPU9250::set_acc_scale(int scale)
{
unsigned int temp_scale;
WriteReg(MPUREG_ACCEL_CONFIG, scale);
switch (scale){
case BITS_FS_2G:
acc_divider=16384;
break;
case BITS_FS_4G:
acc_divider=8192;
break;
case BITS_FS_8G:
acc_divider=4096;
break;
case BITS_FS_16G:
acc_divider=2048;
break;
}
temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
switch (temp_scale){
case BITS_FS_2G:
temp_scale=2;
break;
case BITS_FS_4G:
temp_scale=4;
break;
case BITS_FS_8G:
temp_scale=8;
break;
case BITS_FS_16G:
temp_scale=16;
break;
}
return temp_scale;
}
/*-----------------------------------------------------------------------------------------------
GYROSCOPE SCALE
usage: call this function at startup, after initialization, to set the right range for the
gyroscopes. Suitable ranges are:
BITS_FS_250DPS
BITS_FS_500DPS
BITS_FS_1000DPS
BITS_FS_2000DPS
returns the range set (250,500,1000 or 2000)
-----------------------------------------------------------------------------------------------*/
unsigned int MPU9250::set_gyro_scale(int scale)
{
unsigned int temp_scale;
WriteReg(MPUREG_GYRO_CONFIG, scale);
switch (scale){
case BITS_FS_250DPS:
gyro_divider=131;
break;
case BITS_FS_500DPS:
gyro_divider=65.5;
break;
case BITS_FS_1000DPS:
gyro_divider=32.8;
break;
case BITS_FS_2000DPS:
gyro_divider=16.4;
break;
}
temp_scale=WriteReg(MPUREG_GYRO_CONFIG|READ_FLAG, 0x00);
switch (temp_scale){
case BITS_FS_250DPS:
temp_scale=250;
break;
case BITS_FS_500DPS:
temp_scale=500;
break;
case BITS_FS_1000DPS:
temp_scale=1000;
break;
case BITS_FS_2000DPS:
temp_scale=2000;
break;
}
return temp_scale;
}
/*-----------------------------------------------------------------------------------------------
WHO AM I?
usage: call this function to know if SPI is working correctly. It checks the I2C address of the
mpu9250 which should be 104 when in SPI mode.
returns the I2C address (104)
-----------------------------------------------------------------------------------------------*/
unsigned int MPU9250::whoami()
{
unsigned int response;
response=WriteReg(MPUREG_WHOAMI|READ_FLAG, 0x00);
return response;
}
/*-----------------------------------------------------------------------------------------------
READ ACCELEROMETER
usage: call this function to read accelerometer data. Axis represents selected axis:
0 -> X axis
1 -> Y axis
2 -> Z axis
-----------------------------------------------------------------------------------------------*/
void MPU9250::read_acc()
{
uint8_t response[6];
int16_t bit_data;
float data;
int i;
ReadRegs(MPUREG_ACCEL_XOUT_H,response,6);
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
accelerometer_data[i] = G_SI * data / acc_divider;
}
}
/*-----------------------------------------------------------------------------------------------
READ GYROSCOPE
usage: call this function to read gyroscope data. Axis represents selected axis:
0 -> X axis
1 -> Y axis
2 -> Z axis
-----------------------------------------------------------------------------------------------*/
void MPU9250::read_gyro()
{
uint8_t response[6];
int16_t bit_data;
float data;
int i;
ReadRegs(MPUREG_GYRO_XOUT_H,response,6);
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
gyroscope_data[i] = (PI / 180) * data / gyro_divider;
}
}
/*-----------------------------------------------------------------------------------------------
READ TEMPERATURE
usage: call this function to read temperature data.
returns the value in °C
-----------------------------------------------------------------------------------------------*/
void MPU9250::read_temp()
{
uint8_t response[2];
int16_t bit_data;
float data;
ReadRegs(MPUREG_TEMP_OUT_H,response,2);
bit_data=((int16_t)response[0]<<8)|response[1];
data=(float)bit_data;
temperature=(data/340)+36.53;
}
/*-----------------------------------------------------------------------------------------------
READ ACCELEROMETER CALIBRATION
usage: call this function to read accelerometer data. Axis represents selected axis:
0 -> X axis
1 -> Y axis
2 -> Z axis
returns Factory Trim value
-----------------------------------------------------------------------------------------------*/
void MPU9250::calib_acc()
{
uint8_t response[4];
int temp_scale;
//READ CURRENT ACC SCALE
temp_scale=WriteReg(MPUREG_ACCEL_CONFIG|READ_FLAG, 0x00);
set_acc_scale(BITS_FS_8G);
//ENABLE SELF TEST need modify
//temp_scale=WriteReg(MPUREG_ACCEL_CONFIG, 0x80>>axis);
ReadRegs(MPUREG_SELF_TEST_X,response,4);
calib_data[0]=((response[0]&11100000)>>3)|((response[3]&00110000)>>4);
calib_data[1]=((response[1]&11100000)>>3)|((response[3]&00001100)>>2);
calib_data[2]=((response[2]&11100000)>>3)|((response[3]&00000011));
set_acc_scale(temp_scale);
}
//-----------------------------------------------------------------------------------------------
uint8_t MPU9250::AK8963_whoami(){
uint8_t response;
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_WIA); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Read 1 byte from the magnetometer
//WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
usleep(10000);
response=WriteReg(MPUREG_EXT_SENS_DATA_00|READ_FLAG, 0x00); //Read I2C
//ReadRegs(MPUREG_EXT_SENS_DATA_00,response,1);
//response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
return response;
}
//-----------------------------------------------------------------------------------------------
void MPU9250::calib_mag(){
uint8_t response[3];
float data;
int i;
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_ASAX); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x83); //Read 3 bytes from the magnetometer
//WriteReg(MPUREG_I2C_SLV0_CTRL, 0x81); //Enable I2C and set bytes
usleep(10000);
//response[0]=WriteReg(MPUREG_EXT_SENS_DATA_01|READ_FLAG, 0x00); //Read I2C
ReadRegs(MPUREG_EXT_SENS_DATA_00,response,3);
//response=WriteReg(MPUREG_I2C_SLV0_DO, 0x00); //Read I2C
for(i=0; i<3; i++) {
data=response[i];
magnetometer_ASA[i]=((data-128)/256+1)*Magnetometer_Sensitivity_Scale_Factor;
}
}
//-----------------------------------------------------------------------------------------------
void MPU9250::read_mag(){
uint8_t response[7];
int16_t bit_data;
float data;
int i;
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 6 bytes from the magnetometer
usleep(10000);
ReadRegs(MPUREG_EXT_SENS_DATA_00,response,7);
//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.
for(i=0; i<3; i++) {
bit_data=((int16_t)response[i*2+1]<<8)|response[i*2];
data=(float)bit_data;
magnetometer_data[i]=data*magnetometer_ASA[i];
}
}
//-----------------------------------------------------------------------------------------------
void MPU9250::read_all(){
uint8_t response[21];
int16_t bit_data;
float data;
int i;
//Send I2C command at first
WriteReg(MPUREG_I2C_SLV0_ADDR,AK8963_I2C_ADDR|READ_FLAG); //Set the I2C slave addres of AK8963 and set for read.
WriteReg(MPUREG_I2C_SLV0_REG, AK8963_HXL); //I2C slave 0 register address from where to begin data transfer
WriteReg(MPUREG_I2C_SLV0_CTRL, 0x87); //Read 7 bytes from the magnetometer
//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.
//wait(0.001);
ReadRegs(MPUREG_ACCEL_XOUT_H,response,21);
//Get accelerometer value
for(i=0; i<3; i++) {
bit_data = ((int16_t)response[i*2] << 8)|response[i*2+1];
data = (float)bit_data;
accelerometer_data[i] = G_SI * data / acc_divider;
}
//Get temperature
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
temperature = ((data - 21) / 333.87) + 21;
//Get gyroscope value
for(i=4; i<7; i++) {
bit_data = ((int16_t)response[i*2] << 8) | response[i*2+1];
data = (float)bit_data;
gyroscope_data[i-4] = (PI / 180) * data / gyro_divider;
}
//Get Magnetometer value
for(i=7; i<10; i++) {
bit_data = ((int16_t)response[i*2+1] << 8) | response[i*2];
data = (float)bit_data;
magnetometer_data[i-7] = data * magnetometer_ASA[i-7];
}
}
/*-----------------------------------------------------------------------------------------------
GET VALUES
usage: call this functions to read and get values
returns accel, gyro and mag values
-----------------------------------------------------------------------------------------------*/
void MPU9250::getMotion9(float *ax, float *ay, float *az, float *gx, float *gy, float *gz, float *mx, float *my, float *mz)
{
read_all();
*ax = accelerometer_data[0];
*ay = accelerometer_data[1];
*az = accelerometer_data[2];
*gx = gyroscope_data[0];
*gy = gyroscope_data[1];
*gz = gyroscope_data[2];
*mx = magnetometer_data[0];
*my = magnetometer_data[1];
*mz = magnetometer_data[2];
}
//-----------------------------------------------------------------------------------------------
void MPU9250::getMotion6(float *ax, float *ay, float *az, float *gx, float *gy, float *gz)
{
read_acc();
read_gyro();
*ax = accelerometer_data[0];
*ay = accelerometer_data[1];
*az = accelerometer_data[2];
*gx = gyroscope_data[0];
*gy = gyroscope_data[1];
*gz = gyroscope_data[2];
}

252
darkwater/MPU9250.h Normal file
View File

@@ -0,0 +1,252 @@
/*
Written by Qiyong Mu (kylongmu@msn.com)
Adapted for Raspberry Pi by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com)
*/
#ifndef _MPU9250_H
#define _MPU9250_H
#include "SPIdev.h"
class MPU9250 {
public:
MPU9250();
bool initialize(int sample_rate_div = 1, int low_pass_filter = 0x01);
bool testConnection();
unsigned int WriteReg( uint8_t WriteAddr, uint8_t WriteData );
unsigned int ReadReg( uint8_t WriteAddr, uint8_t WriteData );
void ReadRegs( uint8_t ReadAddr, uint8_t *ReadBuf, unsigned int Bytes );
unsigned int set_gyro_scale(int scale);
unsigned int set_acc_scale(int scale);
void calib_acc();
void calib_mag();
void read_temp();
void read_acc();
void read_gyro();
void read_mag();
void read_all();
unsigned int whoami();
uint8_t AK8963_whoami();
void getMotion9(float *ax, float *ay, float *az, float *gx, float *gy, float *gz, float *mx, float *my, float *mz);
void getMotion6(float *ax, float *ay, float *az, float *gx, float *gy, float *gz);
public:
float acc_divider;
float gyro_divider;
int calib_data[3];
float magnetometer_ASA[3];
float temperature;
float accelerometer_data[3];
float gyroscope_data[3];
float magnetometer_data[3];
private:
float _error;
};
#endif //_MPU9250_H
// MPU9250 registers
#define MPUREG_XG_OFFS_TC 0x00
#define MPUREG_YG_OFFS_TC 0x01
#define MPUREG_ZG_OFFS_TC 0x02
#define MPUREG_X_FINE_GAIN 0x03
#define MPUREG_Y_FINE_GAIN 0x04
#define MPUREG_Z_FINE_GAIN 0x05
#define MPUREG_XA_OFFS_H 0x06
#define MPUREG_XA_OFFS_L 0x07
#define MPUREG_YA_OFFS_H 0x08
#define MPUREG_YA_OFFS_L 0x09
#define MPUREG_ZA_OFFS_H 0x0A
#define MPUREG_ZA_OFFS_L 0x0B
#define MPUREG_PRODUCT_ID 0x0C
#define MPUREG_SELF_TEST_X 0x0D
#define MPUREG_SELF_TEST_Y 0x0E
#define MPUREG_SELF_TEST_Z 0x0F
#define MPUREG_SELF_TEST_A 0x10
#define MPUREG_XG_OFFS_USRH 0x13
#define MPUREG_XG_OFFS_USRL 0x14
#define MPUREG_YG_OFFS_USRH 0x15
#define MPUREG_YG_OFFS_USRL 0x16
#define MPUREG_ZG_OFFS_USRH 0x17
#define MPUREG_ZG_OFFS_USRL 0x18
#define MPUREG_SMPLRT_DIV 0x19
#define MPUREG_CONFIG 0x1A
#define MPUREG_GYRO_CONFIG 0x1B
#define MPUREG_ACCEL_CONFIG 0x1C
#define MPUREG_ACCEL_CONFIG_2 0x1D
#define MPUREG_LP_ACCEL_ODR 0x1E
#define MPUREG_MOT_THR 0x1F
#define MPUREG_FIFO_EN 0x23
#define MPUREG_I2C_MST_CTRL 0x24
#define MPUREG_I2C_SLV0_ADDR 0x25
#define MPUREG_I2C_SLV0_REG 0x26
#define MPUREG_I2C_SLV0_CTRL 0x27
#define MPUREG_I2C_SLV1_ADDR 0x28
#define MPUREG_I2C_SLV1_REG 0x29
#define MPUREG_I2C_SLV1_CTRL 0x2A
#define MPUREG_I2C_SLV2_ADDR 0x2B
#define MPUREG_I2C_SLV2_REG 0x2C
#define MPUREG_I2C_SLV2_CTRL 0x2D
#define MPUREG_I2C_SLV3_ADDR 0x2E
#define MPUREG_I2C_SLV3_REG 0x2F
#define MPUREG_I2C_SLV3_CTRL 0x30
#define MPUREG_I2C_SLV4_ADDR 0x31
#define MPUREG_I2C_SLV4_REG 0x32
#define MPUREG_I2C_SLV4_DO 0x33
#define MPUREG_I2C_SLV4_CTRL 0x34
#define MPUREG_I2C_SLV4_DI 0x35
#define MPUREG_I2C_MST_STATUS 0x36
#define MPUREG_INT_PIN_CFG 0x37
#define MPUREG_INT_ENABLE 0x38
#define MPUREG_ACCEL_XOUT_H 0x3B
#define MPUREG_ACCEL_XOUT_L 0x3C
#define MPUREG_ACCEL_YOUT_H 0x3D
#define MPUREG_ACCEL_YOUT_L 0x3E
#define MPUREG_ACCEL_ZOUT_H 0x3F
#define MPUREG_ACCEL_ZOUT_L 0x40
#define MPUREG_TEMP_OUT_H 0x41
#define MPUREG_TEMP_OUT_L 0x42
#define MPUREG_GYRO_XOUT_H 0x43
#define MPUREG_GYRO_XOUT_L 0x44
#define MPUREG_GYRO_YOUT_H 0x45
#define MPUREG_GYRO_YOUT_L 0x46
#define MPUREG_GYRO_ZOUT_H 0x47
#define MPUREG_GYRO_ZOUT_L 0x48
#define MPUREG_EXT_SENS_DATA_00 0x49
#define MPUREG_EXT_SENS_DATA_01 0x4A
#define MPUREG_EXT_SENS_DATA_02 0x4B
#define MPUREG_EXT_SENS_DATA_03 0x4C
#define MPUREG_EXT_SENS_DATA_04 0x4D
#define MPUREG_EXT_SENS_DATA_05 0x4E
#define MPUREG_EXT_SENS_DATA_06 0x4F
#define MPUREG_EXT_SENS_DATA_07 0x50
#define MPUREG_EXT_SENS_DATA_08 0x51
#define MPUREG_EXT_SENS_DATA_09 0x52
#define MPUREG_EXT_SENS_DATA_10 0x53
#define MPUREG_EXT_SENS_DATA_11 0x54
#define MPUREG_EXT_SENS_DATA_12 0x55
#define MPUREG_EXT_SENS_DATA_13 0x56
#define MPUREG_EXT_SENS_DATA_14 0x57
#define MPUREG_EXT_SENS_DATA_15 0x58
#define MPUREG_EXT_SENS_DATA_16 0x59
#define MPUREG_EXT_SENS_DATA_17 0x5A
#define MPUREG_EXT_SENS_DATA_18 0x5B
#define MPUREG_EXT_SENS_DATA_19 0x5C
#define MPUREG_EXT_SENS_DATA_20 0x5D
#define MPUREG_EXT_SENS_DATA_21 0x5E
#define MPUREG_EXT_SENS_DATA_22 0x5F
#define MPUREG_EXT_SENS_DATA_23 0x60
#define MPUREG_I2C_SLV0_DO 0x63
#define MPUREG_I2C_SLV1_DO 0x64
#define MPUREG_I2C_SLV2_DO 0x65
#define MPUREG_I2C_SLV3_DO 0x66
#define MPUREG_I2C_MST_DELAY_CTRL 0x67
#define MPUREG_SIGNAL_PATH_RESET 0x68
#define MPUREG_MOT_DETECT_CTRL 0x69
#define MPUREG_USER_CTRL 0x6A
#define MPUREG_PWR_MGMT_1 0x6B
#define MPUREG_PWR_MGMT_2 0x6C
#define MPUREG_BANK_SEL 0x6D
#define MPUREG_MEM_START_ADDR 0x6E
#define MPUREG_MEM_R_W 0x6F
#define MPUREG_DMP_CFG_1 0x70
#define MPUREG_DMP_CFG_2 0x71
#define MPUREG_FIFO_COUNTH 0x72
#define MPUREG_FIFO_COUNTL 0x73
#define MPUREG_FIFO_R_W 0x74
#define MPUREG_WHOAMI 0x75
#define MPUREG_XA_OFFSET_H 0x77
#define MPUREG_XA_OFFSET_L 0x78
#define MPUREG_YA_OFFSET_H 0x7A
#define MPUREG_YA_OFFSET_L 0x7B
#define MPUREG_ZA_OFFSET_H 0x7D
#define MPUREG_ZA_OFFSET_L 0x7E
/* ---- AK8963 Reg In MPU9250 ----------------------------------------------- */
#define AK8963_I2C_ADDR 0x0c//0x18
#define AK8963_Device_ID 0x48
// Read-only Reg
#define AK8963_WIA 0x00
#define AK8963_INFO 0x01
#define AK8963_ST1 0x02
#define AK8963_HXL 0x03
#define AK8963_HXH 0x04
#define AK8963_HYL 0x05
#define AK8963_HYH 0x06
#define AK8963_HZL 0x07
#define AK8963_HZH 0x08
#define AK8963_ST2 0x09
// Write/Read Reg
#define AK8963_CNTL1 0x0A
#define AK8963_CNTL2 0x0B
#define AK8963_ASTC 0x0C
#define AK8963_TS1 0x0D
#define AK8963_TS2 0x0E
#define AK8963_I2CDIS 0x0F
// Read-only Reg ( ROM )
#define AK8963_ASAX 0x10
#define AK8963_ASAY 0x11
#define AK8963_ASAZ 0x12
// Configuration bits MPU9250
#define BIT_SLEEP 0x40
#define BIT_H_RESET 0x80
#define BITS_CLKSEL 0x07
#define MPU_CLK_SEL_PLLGYROX 0x01
#define MPU_CLK_SEL_PLLGYROZ 0x03
#define MPU_EXT_SYNC_GYROX 0x02
#define BITS_FS_250DPS 0x00
#define BITS_FS_500DPS 0x08
#define BITS_FS_1000DPS 0x10
#define BITS_FS_2000DPS 0x18
#define BITS_FS_2G 0x00
#define BITS_FS_4G 0x08
#define BITS_FS_8G 0x10
#define BITS_FS_16G 0x18
#define BITS_FS_MASK 0x18
#define BITS_DLPF_CFG_256HZ_NOLPF2 0x00
#define BITS_DLPF_CFG_188HZ 0x01
#define BITS_DLPF_CFG_98HZ 0x02
#define BITS_DLPF_CFG_42HZ 0x03
#define BITS_DLPF_CFG_20HZ 0x04
#define BITS_DLPF_CFG_10HZ 0x05
#define BITS_DLPF_CFG_5HZ 0x06
#define BITS_DLPF_CFG_2100HZ_NOLPF 0x07
#define BITS_DLPF_CFG_MASK 0x07
#define BIT_INT_ANYRD_2CLEAR 0x10
#define BIT_RAW_RDY_EN 0x01
#define BIT_I2C_IF_DIS 0x10
#define READ_FLAG 0x80
/* ---- Sensitivity --------------------------------------------------------- */
#define MPU9250A_2g ((float)0.000061035156f) // 0.000061035156 g/LSB
#define MPU9250A_4g ((float)0.000122070312f) // 0.000122070312 g/LSB
#define MPU9250A_8g ((float)0.000244140625f) // 0.000244140625 g/LSB
#define MPU9250A_16g ((float)0.000488281250f) // 0.000488281250 g/LSB
#define MPU9250G_250dps ((float)0.007633587786f) // 0.007633587786 dps/LSB
#define MPU9250G_500dps ((float)0.015267175572f) // 0.015267175572 dps/LSB
#define MPU9250G_1000dps ((float)0.030487804878f) // 0.030487804878 dps/LSB
#define MPU9250G_2000dps ((float)0.060975609756f) // 0.060975609756 dps/LSB
#define MPU9250M_4800uT ((float)0.6f) // 0.6 uT/LSB
#define MPU9250T_85degC ((float)0.002995177763f) // 0.002995177763 degC/LSB
#define Magnetometer_Sensitivity_Scale_Factor ((float)0.15f)

183
darkwater/PCA9685.cpp Normal file
View File

@@ -0,0 +1,183 @@
/*
PCA9685 driver code is placed under the BSD license.
Written by Team Dark Water (team@darkwater.io) based on the Adafruit Python Library and the Emlid PCA9685 library
Copyright (c) 2014, Dark Water
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Emlid Limited nor the names of its contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#include "PCA9685.h"
/** PCA9685 constructor.
* @param address I2C address
* @see PCA9685_DEFAULT_ADDRESS
*/
PCA9685::PCA9685(uint8_t address) {
this->devAddr = address;
}
/** Power on and prepare for general usage.
* This method reads prescale value stored in PCA9685 and calculate frequency based on it.
* Then it enables auto-increment of register address to allow for faster writes.
* And finally the restart is performed to enable clocking.
*/
void PCA9685::initialize() {
this->frequency = getFrequency();
I2Cdev::writeBit(devAddr, PCA9685_RA_MODE1, PCA9685_MODE1_AI_BIT, 1);
restart();
}
/** Verify the I2C connection.
* @return True if connection is valid, false otherwise
*/
bool PCA9685::testConnection() {
uint8_t data;
int8_t status = I2Cdev::readByte(devAddr, PCA9685_RA_PRE_SCALE, &data);
if (status > 0)
return true;
else
return false;
}
/** Put PCA9685 to sleep mode thus turning off the outputs.
* @see PCA9685_MODE1_SLEEP_BIT
*/
void PCA9685::sleep() {
I2Cdev::writeBit(devAddr, PCA9685_RA_MODE1, PCA9685_MODE1_SLEEP_BIT, 1);
}
/** Disable sleep mode and start the outputs.
* @see PCA9685_MODE1_SLEEP_BIT
* @see PCA9685_MODE1_RESTART_BIT
*/
void PCA9685::restart() {
I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x00);
I2Cdev::writeByte(devAddr, PCA9685_RA_MODE2, 0x04);
}
/** Calculate prescale value based on the specified frequency and write it to the device.
* @return Frequency in Hz
* @see PCA9685_RA_PRE_SCALE
*/
float PCA9685::getFrequency() {
uint8_t data;
I2Cdev::readByte(devAddr, PCA9685_RA_PRE_SCALE, &data);
return 24576000.f / 4096.f / (data + 1);
}
/** Calculate prescale value based on the specified frequency and write it to the device.
* @param Frequency in Hz
* @see PCA9685_RA_PRE_SCALE
*/
void PCA9685::setFrequency(float frequency) {
sleep();
usleep(10000);
uint8_t prescale = roundf(25000000.f / 4096.f / frequency) - 1;
I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x10);
I2Cdev::writeByte(devAddr, PCA9685_RA_PRE_SCALE, prescale);
I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x80);
I2Cdev::writeByte(devAddr, PCA9685_RA_MODE1, 0x04);
this->frequency = getFrequency();
restart();
}
/** Set channel start offset of the pulse and it's length
* @param Channel number (0-15)
* @param Offset (0-4095)
* @param Length (0-4095)
* @see PCA9685_RA_LED0_ON_L
*/
void PCA9685::setPWM(uint8_t channel, uint16_t offset, uint16_t length) {
I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_ON_L + LED_MULTIPLYER * channel, offset & 0xFF);
I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_ON_H + LED_MULTIPLYER * channel, offset >> 8);
I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_OFF_L + LED_MULTIPLYER * channel, length & 0xFF);
I2Cdev::writeByte(devAddr, PCA9685_RA_LED0_OFF_H + LED_MULTIPLYER * channel, length >> 8);
}
/** Set channel's pulse length
* @param Channel number (0-15)
* @param Length (0-4095)
* @see PCA9685_RA_LED0_ON_L
*/
void PCA9685::setPWM(uint8_t channel, uint16_t length) {
setPWM(channel, 0, length);
}
/** Set channel's pulse length in milliseconds
* @param Channel number (0-15)
* @param Length in milliseconds
* @see PCA9685_RA_LED0_ON_L
*/
void PCA9685::setPWMmS(uint8_t channel, float length_mS) {
setPWM(channel, round((length_mS * 4096.f) / (1000.f / frequency)));
}
/** Set channel's pulse length in microseconds
* @param Channel number (0-15)
* @param Length in microseconds
* @see PCA9685_RA_LED0_ON_L
*/
void PCA9685::setPWMuS(uint8_t channel, float length_uS) {
setPWM(channel, round((length_uS * 4096.f) / (1000000.f / frequency)));
}
/** Set start offset of the pulse and it's length for all channels
* @param Offset (0-4095)
* @param Length (0-4095)
* @see PCA9685_RA_ALL_LED_ON_L
*/
void PCA9685::setAllPWM(uint16_t offset, uint16_t length) {
I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_ON_L, offset & 0xFF);
I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_ON_H, offset >> 8);
I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_OFF_L, length & 0xFF);
I2Cdev::writeByte(devAddr, PCA9685_RA_ALL_LED_OFF_H, length >> 8);
}
/** Set pulse length for all channels
* @param Length (0-4095)
* @see PCA9685_RA_ALL_LED_ON_L
*/
void PCA9685::setAllPWM(uint16_t length) {
setAllPWM(0, length);
}
/** Set pulse length in milliseconds for all channels
* @param Length in milliseconds
* @see PCA9685_RA_ALL_LED_ON_L
*/
void PCA9685::setAllPWMmS(float length_mS) {
setAllPWM(round((length_mS * 4096.f) / (1000.f / frequency)));
}
/** Set pulse length in microseconds for all channels
* @param Length in microseconds
* @see PCA9685_RA_ALL_LED_ON_L
*/
void PCA9685::setAllPWMuS(float length_uS) {
setAllPWM(round((length_uS * 4096.f) / (1000000.f / frequency)));
}

104
darkwater/PCA9685.h Normal file
View File

@@ -0,0 +1,104 @@
/*
PCA9685 driver code is placed under the BSD license.
Written by Team Dark Water (team@darkwater.io) based on the Adafruit Python Library and the Emlid PCA9685 library
Copyright (c) 2014, Dark Water
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Emlid Limited nor the names of its contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef PCA9685_HPP
#define PCA9685_HPP
#include <stdint.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <math.h>
#include <string>
#include "I2Cdev.h"
#define PCA9685_DEFAULT_ADDRESS 0x60 // 640 default / ESCAPE default is 61
#define PCA9685_RA_MODE1 0x00
#define PCA9685_RA_MODE2 0x01
#define PCA9685_RA_SUBADR1 0x02 //I2C-bus subaddress 1
#define PCA9685_RA_SUBADR2 0x03 //I2C-bus subaddress 2
#define PCA9685_RA_SUBADR3 0x04 //I2C-bus subaddress 3
#define PCA9685_RA_ALLCALLADR 0x05 //LED All Call I2C-bus address
#define PCA9685_RA_LED0_ON_L 0x06
#define PCA9685_RA_LED0_ON_H 0x07
#define PCA9685_RA_LED0_OFF_L 0x08
#define PCA9685_RA_LED0_OFF_H 0x09
#define LED_MULTIPLYER 4 // For the other 15 channels
#define PCA9685_RA_ALL_LED_ON_L 0xFA
#define PCA9685_RA_ALL_LED_ON_H 0xFB
#define PCA9685_RA_ALL_LED_OFF_L 0xFC
#define PCA9685_RA_ALL_LED_OFF_H 0xFD
#define PCA9685_RA_PRE_SCALE 0xFE
#define PCA9685_MODE1_RESTART_BIT 7
#define PCA9685_MODE1_EXTCLK_BIT 6
#define PCA9685_MODE1_AI_BIT 5
#define PCA9685_MODE1_SLEEP_BIT 4
#define PCA9685_MODE1_SUB1_BIT 3
#define PCA9685_MODE1_SUB2_BIT 2
#define PCA9685_MODE1_SUB3_BIT 1
#define PCA9685_MODE1_ALLCALL_BIT 0
#define PCA9685_MODE2_INVRT_BIT 4
#define PCA9685_MODE2_OCH_BIT 3
#define PCA9685_MODE2_OUTDRV_BIT 2
#define PCA9685_MODE2_OUTNE1_BIT 1
#define PCA9685_MODE2_OUTNE0_BIT 0
class PCA9685 {
public:
PCA9685(uint8_t address = PCA9685_DEFAULT_ADDRESS);
void initialize();
bool testConnection();
float getFrequency();
void setFrequency(float frequency);
void sleep();
void restart();
void setPWM(uint8_t channel, uint16_t offset, uint16_t length);
void setPWM(uint8_t channel, uint16_t length);
void setPWMmS(uint8_t channel, float length_mS);
void setPWMuS(uint8_t channel, float length_uS);
void setAllPWM(uint16_t offset, uint16_t length);
void setAllPWM(uint16_t length);
void setAllPWMmS(float length_mS);
void setAllPWMuS(float length_uS);
private:
uint8_t devAddr;
float frequency;
};
#endif // PCA9685_ HPP

99
darkwater/SPIdev.h Normal file
View File

@@ -0,0 +1,99 @@
/*
SPIDev driver code is placed under the BSD license.
Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com)
Copyright (c) 2014, Emlid Limited
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
* Redistributions in binary form must reproduce the above copyright
notice, this list of conditions and the following disclaimer in the
documentation and/or other materials provided with the distribution.
* Neither the name of the Emlid Limited nor the names of its contributors
may be used to endorse or promote products derived from this software
without specific prior written permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL EMLID LIMITED BE LIABLE FOR ANY
DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
(INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
(INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef _SPIDEV_H_
#define _SPIDEV_H_
//#define _XOPEN_SOURCE 600
#include <unistd.h>
#include <fcntl.h>
#include <sys/ioctl.h>
#include <linux/types.h>
#include <linux/spi/spidev.h>
#include <sys/socket.h>
#include <netdb.h>
#include <cstdlib>
#include <cstring>
#include <cstdio>
class SPIdev {
public:
SPIdev()
{
}
static int transfer(const char *spidev,
unsigned char *tx,
unsigned char *rx,
unsigned int length,
unsigned int speed_hz = 1000000,
unsigned char bits_per_word = 8,
unsigned short delay_usecs = 0)
{
spi_ioc_transfer spi_transfer;
memset(&spi_transfer, 0, sizeof(spi_ioc_transfer));
spi_transfer.tx_buf = (unsigned long)tx;
spi_transfer.rx_buf = (unsigned long)rx;
spi_transfer.len = length;
spi_transfer.speed_hz = speed_hz;
spi_transfer.bits_per_word = bits_per_word;
spi_transfer.delay_usecs = delay_usecs;
int spi_fd = ::open(spidev, O_RDWR);
if (spi_fd < 0 ) {
printf("Error: Can not open SPI device\n");
return -1;
}
int status = ioctl(spi_fd, SPI_IOC_MESSAGE(1), &spi_transfer);
::close(spi_fd);
// Debug information
/*
printf("Tx: ");
for (int i = 0; i < length; i++)
printf("%x ", tx[i]);
printf("\n");
printf("Rx: ");
for (int i = 0; i < length; i++)
printf("%x ", rx[i]);
printf("\n");
*/
return status;
}
};
#endif //_SPIDEV_H_

70
darkwater/Util.cpp Normal file
View File

@@ -0,0 +1,70 @@
#include <cstdio>
#include <stdarg.h>
#include <stdlib.h>
#include <errno.h>
#include <fcntl.h>
#include <unistd.h>
#include "Util.h"
#define SCRIPT_PATH "../../../check_apm.sh"
int write_file(const char *path, const char *fmt, ...)
{
errno = 0;
int fd = ::open(path, O_WRONLY | O_CLOEXEC);
if (fd == -1) {
return -errno;
}
va_list args;
va_start(args, fmt);
int ret = ::vdprintf(fd, fmt, args);
int errno_bkp = errno;
::close(fd);
va_end(args);
if (ret < 1) {
return -errno_bkp;
}
return ret;
}
int read_file(const char *path, const char *fmt, ...)
{
errno = 0;
FILE *file = ::fopen(path, "re");
if (!file)
return -errno;
va_list args;
va_start(args, fmt);
int ret = ::vfscanf(file, fmt, args);
int errno_bkp = errno;
::fclose(file);
va_end(args);
if (ret < 1)
return -errno_bkp;
return ret;
}
bool check_apm()
{
int ret = system("ps -AT | grep -c sched-timer > /dev/null");
if (WEXITSTATUS(ret) <= 0) {
fprintf(stderr, "APM is running. Can't launch the example\n");
return true;
}
return false;
}

7
darkwater/Util.h Normal file
View File

@@ -0,0 +1,7 @@
#pragma once
#define ARRAY_SIZE(a) sizeof(a) / sizeof(a[0])
int write_file(const char *path, const char *fmt, ...);
int read_file(const char *path, const char *fmt, ...);
bool check_apm();

160
darkwater/gpio.cpp Normal file
View File

@@ -0,0 +1,160 @@
#include <unistd.h>
#include <fcntl.h>
#include <poll.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <err.h>
#include <cstdio>
#include <cstdlib>
#include <cstring>
#include "gpio.h"
#define LOW 0
#define HIGH 1
/* Raspberry Pi GPIO memory */
#define BCM2708_PERI_BASE 0x20000000
#define BCM2709_PERI_BASE 0x3F000000
#define GPIO_BASE(address) (address + 0x200000)
#define PAGE_SIZE (4*1024)
#define BLOCK_SIZE (4*1024)
/* GPIO setup. Always use INP_GPIO(x) before OUT_GPIO(x) or SET_GPIO_ALT(x,y) */
#define GPIO_MODE_IN(g) *(_gpio+((g)/10)) &= ~(7<<(((g)%10)*3))
#define GPIO_MODE_OUT(g) *(_gpio+((g)/10)) |= (1<<(((g)%10)*3))
#define GPIO_MODE_ALT(g,a) *(_gpio+(((g)/10))) |= (((a)<=3?(a)+4:(a)==4?3:2)<<(((g)%10)*3))
#define GPIO_SET_HIGH *(_gpio+7) // sets bits which are 1
#define GPIO_SET_LOW *(_gpio+10) // clears bits which are 1
#define GPIO_GET(g) (*(_gpio+13)&(1<<g)) // 0 if LOW, (1<<g) if HIGH
#define MAX_SIZE_LINE 50
using namespace DarkWater;
Pin::Pin(uint8_t pin):
_pin(pin),
_gpio(NULL),
_mode(GpioModeInput)
{
}
Pin::~Pin()
{
if (!_deinit()) {
warnx("deinitialization failed");
}
}
bool Pin::_deinit()
{
if (munmap(const_cast<uint32_t *>(_gpio), BLOCK_SIZE) < 0) {
warnx("unmap failed");
return false;
}
return true;
}
bool Pin::init()
{
int mem_fd;
if ((mem_fd = open("/dev/mem", O_RDWR|O_SYNC) ) < 0) {
warn("/dev/mem cannot be opened");
return false;
}
uint32_t address = getRaspberryPiVersion() == 1? GPIO_BASE(BCM2708_PERI_BASE): GPIO_BASE(BCM2709_PERI_BASE);
void *gpio_map = mmap(
NULL, /* any adddress in our space will do */
BLOCK_SIZE, /* map length */
PROT_READ|PROT_WRITE, /* enable reading & writting to mapped memory */
MAP_SHARED, /* shared with other processes */
mem_fd, /* file to map */
address /* offset to GPIO peripheral */
);
if (gpio_map == MAP_FAILED) {
warn("cannot mmap memory");
return false;
}
/* No need to keep mem_fd open after mmap */
if (close(mem_fd) < 0) {
warn("cannot close mem_fd");
return false;
}
_gpio = reinterpret_cast<volatile uint32_t *>(gpio_map); // Always use volatile pointer!
return true;
}
void Pin::setMode(GpioMode mode)
{
if (mode == GpioModeInput) {
GPIO_MODE_IN(_pin);
} else {
GPIO_MODE_IN(_pin);
GPIO_MODE_OUT(_pin);
}
_mode = mode;
}
uint8_t Pin::read() const
{
uint32_t value = GPIO_GET(_pin);
return value ? 1: 0;
}
void Pin::write(uint8_t value)
{
if (_mode != GpioModeOutput) {
warnx("no effect because mode is not set");
}
if (value == LOW) {
GPIO_SET_LOW = 1 << _pin;
} else {
GPIO_SET_HIGH = 1 << _pin;
}
}
void Pin::toggle()
{
write(!read());
}
int Pin::getRaspberryPiVersion() const
{
char buffer[MAX_SIZE_LINE];
const char* hardware_description_entry = "Hardware";
const char* v1 = "BCM2708";
const char* v2 = "BCM2709";
char* flag;
FILE* fd;
fd = fopen("/proc/cpuinfo", "r");
while (fgets(buffer, MAX_SIZE_LINE, fd) != NULL) {
flag = strstr(buffer, hardware_description_entry);
if (flag != NULL) {
if (strstr(buffer, v2) != NULL) {
fclose(fd);
return 2;
} else if (strstr(buffer, v1) != NULL) {
fclose(fd);
return 1;
}
}
}
/* defaults to 1 */
fprintf(stderr, "Could not detect RPi version, defaulting to 1\n");
fclose(fd);
return 1;
}

62
darkwater/gpio.h Normal file
View File

@@ -0,0 +1,62 @@
#ifndef __DARKWATER_GPIO_H__
#define __DARKWATER_GPIO_H__
#include <stdint.h>
namespace DarkWater {
/* Raspberry Pi GPIO mapping */
#define RPI_GPIO_2 2 // Pin 3 SDA
#define RPI_GPIO_3 3 // Pin 5 SCL
#define RPI_GPIO_4 4 // Pin 7 NAVIO_PPM_INPUT
#define RPI_GPIO_7 7 // Pin 26 CE1 NAVIO_MPU9250_CS
#define RPI_GPIO_8 8 // Pin 24 CE0 NAVIO_UBLOX_CS
#define RPI_GPIO_9 9 // Pin 21 MISO
#define RPI_GPIO_10 10 // Pin 19 MOSI
#define RPI_GPIO_11 11 // Pin 23 SCLK
#define RPI_GPIO_14 14 // Pin 8 TxD
#define RPI_GPIO_15 15 // Pin 10 RxD
#define RPI_GPIO_17 17 // Pin 11 NAVIO_UART_PORT_5
#define RPI_GPIO_18 18 // Pin 12 NAVIO_UART_PORT_4
#define RPI_GPIO_22 22 // Pin 15 NAVIO_UBLOX_PPS
#define RPI_GPIO_23 23 // Pin 16 NAVIO_MPU9250_DRDY
#define RPI_GPIO_24 24 // Pin 18 NAVIO_SPI_PORT_6
#define RPI_GPIO_25 25 // Pin 22 NAVIO_SPI_PORT_5
#define RPI_GPIO_27 27 // Pin 13 NAVIO_PCA9685_OE
#define RPI_GPIO_28 28 // Pin 3
#define RPI_GPIO_29 29 // Pin 4
#define RPI_GPIO_30 30 // Pin 5
#define RPI_GPIO_31 31 // Pin 6
class Pin {
public:
typedef enum {
GpioModeInput,
GpioModeOutput
} GpioMode;
Pin(uint8_t pin);
~Pin();
bool init();
void setMode(GpioMode mode);
uint8_t read() const;
void write(uint8_t value);
void toggle();
private:
int getRaspberryPiVersion() const;
Pin (const Pin&);
Pin& operator=(const Pin&);
uint8_t _pin;
volatile uint32_t *_gpio;
GpioMode _mode;
bool _deinit();
};
}
#endif // __DARKWATER_GPIO_H__

41
examples/AccelGyroMag/AccelGyroMag.cpp Normal file
View File

@@ -0,0 +1,41 @@
/*
Provided to you by Emlid Ltd (c) 2014.
twitter.com/emlidtech || www.emlid.com || info@emlid.com
Example: Read accelerometer, gyroscope and magnetometer values from
MPU9250 inertial measurement unit over SPI on Raspberry Pi + Navio.
Navio's onboard MPU9250 is connected to the SPI bus on Raspberry Pi
and can be read through /dev/spidev0.1
To run this example navigate to the directory containing it and run following commands:
make
./AccelGyroMag
*/
#include "darkwater/MPU9250.h"
#include "darkwater/Util.h"
#include <stdlib.h>
//=============================================================================
int main()
{
if (check_apm()) {
return 1;
}
//-------------------------------------------------------------------------
MPU9250 imu;
imu.initialize();
float ax, ay, az, gx, gy, gz, mx, my, mz;
//-------------------------------------------------------------------------
while(1) {
imu.getMotion9(&ax, &ay, &az, &gx, &gy, &gz, &mx, &my, &mz);
printf("Acc: %+7.3f %+7.3f %+7.3f ", ax, ay, az);
printf("Gyr: %+8.3f %+8.3f %+8.3f ", gx, gy, gz);
printf("Mag: %+7.3f %+7.3f %+7.3f\n", mx, my, mz);
usleep(500000);
}}

9
examples/AccelGyroMag/Makefile Normal file
View File

@@ -0,0 +1,9 @@
CC = g++
DW = ../../darkwater
INCLUDES = -I ../..
all:
$(CC) $(INCLUDES) AccelGyroMag.cpp $(DW)/MPU9250.cpp $(DW)/Util.cpp -o AccelGyroMag
clean:
rm AccelGyroMag

15
examples/Makefile Normal file
View File

@@ -0,0 +1,15 @@
MODULES = AccelGyroMag ADC AHRS Barometer FRAM GPS LED Multithread PPM-decoder Servo
all:
for dir in $(MODULES); do \
cd $$dir; \
($(MAKE) ); \
cd ..; \
done
clean:
for dir in $(MODULES); do \
cd $$dir; \
($(MAKE) clean); \
cd ..; \
done

9
examples/Motor/Makefile Normal file
View File

@@ -0,0 +1,9 @@
CC = g++
DW = ../../darkwater
INCLUDES = -I ../..
all:
$(CC) $(INCLUDES) Motor.cpp $(DW)/DW640.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o Motor
clean:
rm Motor

110
examples/Motor/Motor.cpp Normal file
View File

@@ -0,0 +1,110 @@
/*
Provided to you by Emlid Ltd (c) 2014.
twitter.com/emlidtech || www.emlid.com || info@emlid.com
Example: Read accelerometer, gyroscope and magnetometer values from
MPU9250 inertial measurement unit over SPI on Raspberry Pi + Navio.
Navio's onboard MPU9250 is connected to the SPI bus on Raspberry Pi
and can be read through /dev/spidev0.1
To run this example navigate to the directory containing it and run following commands:
make
./AccelGyroMag
*/
#include "darkwater/DW640.h"
#include "darkwater/Util.h"
#include <stdlib.h>
//=============================================================================
int main()
{
if (check_apm()) {
return 1;
}
//-------------------------------------------------------------------------
DW640 dw;
dw.initialize();
DW_Motor *dw1 = dw.getMotor(1);
DW_Motor *dw2 = dw.getMotor(2);
DW_Motor *dw3 = dw.getMotor(3);
DW_Motor *dw4 = dw.getMotor(4);
DW_Motor *dw5 = dw.getMotor(5);
DW_Motor *dw6 = dw.getMotor(6);
dw1->off();
dw2->off();
dw3->off();
dw4->off();
dw5->off();
dw6->off();
usleep(1000000);
printf("Set forward - \n");
printf("Motor 1\n");
dw1->setMotorSpeed( 255 );
usleep(1000000);
printf("Motor 2\n");
dw2->setMotorSpeed( 255 );
usleep(1000000);
printf("Motor 3\n");
dw3->setMotorSpeed( 255 );
usleep(1000000);
printf("Motor 4\n");
dw4->setMotorSpeed( 255 );
usleep(1000000);
printf("Motor 5\n");
dw5->setMotorSpeed( 255 );
usleep(1000000);
printf("Motor 6\n");
dw6->setMotorSpeed( 255 );
usleep(1000000);
printf("Stopping - \n");
printf("Motor 1\n");
dw1->setMotorSpeed( 0 );
usleep(1000000);
printf("Motor 2\n");
dw2->setMotorSpeed( 0 );
usleep(1000000);
printf("Motor 3\n");
dw3->setMotorSpeed( 0 );
usleep(1000000);
printf("Motor 4\n");
dw4->setMotorSpeed( 0 );
usleep(1000000);
printf("Motor 5\n");
dw5->setMotorSpeed( 0 );
usleep(1000000);
printf("Motor 6\n");
dw6->setMotorSpeed( 0 );
usleep(1000000);
printf("Set reverse - \n");
printf("Motor 1\n");
dw1->setMotorSpeed( -255 );
usleep(1000000);
printf("Motor 2\n");
dw2->setMotorSpeed( -255 );
usleep(1000000);
printf("Motor 3\n");
dw3->setMotorSpeed( -255 );
usleep(1000000);
printf("Motor 4\n");
dw4->setMotorSpeed( -255 );
usleep(1000000);
printf("Motor 5\n");
dw5->setMotorSpeed( -255 );
usleep(1000000);
printf("Motor 6\n");
dw6->setMotorSpeed( -255 );
usleep(1000000);
printf("All off \n");
dw1->off();
dw2->off();
dw3->off();
dw4->off();
dw5->off();
dw6->off();
}

9
examples/Multithread/Makefile Normal file
View File

@@ -0,0 +1,9 @@
CC = g++
NAVIO = ../../Navio
INCLUDES = -I ../..
all:
$(CC) $(INCLUDES) threaded_baro.cpp $(NAVIO)/MS5611.cpp $(NAVIO)/I2Cdev.cpp $(NAVIO)/Util.cpp -lpthread -o threaded_baro
clean:
rm threaded_baro

66
examples/Multithread/threaded_baro.cpp Normal file
View File

@@ -0,0 +1,66 @@
/*
Provided to you by Emlid Ltd (c) 2014.
twitter.com/emlidtech || www.emlid.com || info@emlid.com
Example: Get pressure from MS5611 barometer onboard of Navio shield for Raspberry Pi
using a different thread, to update pressure info in the background
To run this example navigate to the directory containing it and run following commands:
make
sudo ./threaded_baro
*/
#include "darkwater/MS5611.h"
#include "darkwater/Util.h"
#include <unistd.h>
#include <stdio.h>
#include <pthread.h>
void * acquireBarometerData(void * barom)
{
MS5611* barometer = (MS5611*)barom;
while (true) {
barometer->refreshPressure();
usleep(10000); // Waiting for pressure data ready
barometer->readPressure();
barometer->refreshTemperature();
usleep(10000); // Waiting for temperature data ready
barometer->readTemperature();
barometer->calculatePressureAndTemperature();
//sleep(0.5);
}
pthread_exit(NULL);
}
int main()
{
if (check_apm()) {
return 1;
}
MS5611 baro;
baro.initialize();
pthread_t baro_thread;
if(pthread_create(&baro_thread, NULL, acquireBarometerData, (void *)&baro))
{
printf("Error: Failed to create barometer thread\n");
return 0;
}
while(true)
{
printf("Temperature(C): %f Pressure(millibar): %f\n", baro.getTemperature(), baro.getPressure());
sleep(1);
}
pthread_exit(NULL);
return 1;
}

14
examples/PPM-decoder/Makefile Normal file
View File

@@ -0,0 +1,14 @@
CC = g++
DW = ../../darkwater
PIGPIO_PATH := $(PIGPIO_PATH)
LIB = -L$(PIGPIO_PATH)
INCLUDES = -I ../.. -I$(PIGPIO_PATH)
all:
$(CC) $(INCLUDES) $(LIB) PPM.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o PPM -lrt -lpthread -lpigpio
clean:
rm PPM

108
examples/PPM-decoder/PPM.cpp Normal file
View File

@@ -0,0 +1,108 @@
/*
This code is provided under the BSD license.
Copyright (c) 2014, Emlid Limited. All rights reserved.
Written by Mikhail Avkhimenia (mikhail.avkhimenia@emlid.com)
twitter.com/emlidtech || www.emlid.com || info@emlid.com
Application: PPM to PWM decoder for Raspberry Pi with Navio.
Requres pigpio library, please install it first - http://abyz.co.uk/rpi/pigpio/
To run this app navigate to the directory containing it and run following commands:
make
sudo ./PPM
*/
#include <pigpio.h>
#include <stdio.h>
#include <unistd.h>
#include <darkwater/gpio.h>
#include "darkwater/PCA9685.h"
#include "darkwater/Util.h"
//================================ Options =====================================
unsigned int samplingRate = 1; // 1 microsecond (can be 1,2,4,5,10)
unsigned int ppmInputGpio = 4; // PPM input on Navio's 2.54 header
unsigned int ppmSyncLength = 4000; // Length of PPM sync pause
unsigned int ppmChannelsNumber = 8; // Number of channels packed in PPM
unsigned int servoFrequency = 50; // Servo control frequency
bool verboseOutputEnabled = true; // Output channels values to console
//============================ Objects & data ==================================
PCA9685 *pwm;
float channels[8];
//============================== PPM decoder ===================================
unsigned int currentChannel = 0;
unsigned int previousTick;
unsigned int deltaTime;
void ppmOnEdge(int gpio, int level, uint32_t tick)
{
if (level == 0) {
deltaTime = tick - previousTick;
previousTick = tick;
if (deltaTime >= ppmSyncLength) { // Sync
currentChannel = 0;
// RC output
for (int i = 0; i < ppmChannelsNumber; i++)
pwm->setPWMuS(i + 3, channels[i]); // 1st Navio RC output is 3
// Console output
if (verboseOutputEnabled) {
printf("\n");
for (int i = 0; i < ppmChannelsNumber; i++)
printf("%4.f ", channels[i]);
}
}
else
if (currentChannel < ppmChannelsNumber)
channels[currentChannel++] = deltaTime;
}
}
//================================== Main ======================================
using namespace Navio;
int main(int argc, char *argv[])
{
static const uint8_t outputEnablePin = RPI_GPIO_27;
if (check_apm()) {
return 1;
}
Pin pin(outputEnablePin);
if (pin.init()) {
pin.setMode(Pin::GpioModeOutput);
pin.write(0); /* drive Output Enable low */
} else {
fprintf(stderr, "Output Enable not set. Are you root?");
}
// Servo controller setup
pwm = new PCA9685();
pwm->initialize();
pwm->setFrequency(servoFrequency);
// GPIO setup
gpioCfgClock(samplingRate, PI_DEFAULT_CLK_PERIPHERAL, 0); /* last parameter is deprecated now */
gpioInitialise();
previousTick = gpioTick();
gpioSetAlertFunc(ppmInputGpio, ppmOnEdge);
// Infinite sleep - all action is now happening in ppmOnEdge() function
while(1)
sleep(10);
return 0;
}

9
examples/Servo/Makefile Normal file
View File

@@ -0,0 +1,9 @@
CC = g++
DW = ../../darkwater
INCLUDES = -I ../..
all:
$(CC) $(INCLUDES) Servo.cpp $(DW)/DW640.cpp $(DW)/PCA9685.cpp $(DW)/I2Cdev.cpp $(DW)/gpio.cpp $(DW)/Util.cpp -o Servo
clean:
rm Servo

42
examples/Servo/Servo.cpp Normal file
View File

@@ -0,0 +1,42 @@
#define SERVO_MIN 1.250 /*mS*/
#define SERVO_MAX 1.750 /*mS*/
#include "darkwater/DW640.h"
#include "darkwater/Util.h"
#include <stdlib.h>
using namespace DarkWater;
int main()
{
if (check_apm()) {
return 1;
}
DW640 dw;
dw.initialize();
dw.setFrequency(50);
DW_Servo *s1 = dw.getServo(1);
DW_Servo *s2 = dw.getServo(2);
s1->off();
s2->off();
printf("Start servo moves\n");
for( int a = 10; a >= 0; a-- ) {
printf("Step %d\n", a);
s1->setPWMmS(SERVO_MIN);
s2->setPWMmS(SERVO_MIN);
usleep(1000000);
s1->setPWMmS(SERVO_MAX);
s2->setPWMmS(SERVO_MAX);
usleep(1000000);
}
s1->off();
s2->off();
return 0;
}