Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'master' of https://code.google.com/p/ardupilot-mega

This commit is contained in:
Chris Anderson 2011-12-28 08:53:19 -08:00
parent 04c73fbe72 671d0b82ad
commit 95c2692982
33 changed files with 1112 additions and 323 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
ArduCopter/ArduCopter.pde
View File

@ -54,7 +54,7 @@ http://code.google.com/p/ardupilot-mega/downloads/list
#include <Arduino_Mega_ISR_Registry.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib
#include <I2C.h> // Arduino I2C lib
#include <SPI.h> // Arduino SPI lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
@ -703,9 +703,10 @@ static void medium_loop()
#if HIL_MODE != HIL_MODE_ATTITUDE // don't execute in HIL mode
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
}
#endif

1
ArduCopter/config.h
View File

@ -52,6 +52,7 @@
# define CONFIG_RELAY DISABLED
# define MAG_ORIENTATION AP_COMPASS_APM2_SHIELD
# define CONFIG_SONAR_SOURCE SONAR_SOURCE_ANALOG_PIN
# define MAGNETOMETER ENABLED
# ifdef APM2_BETA_HARDWARE
# define CONFIG_BARO AP_BARO_BMP085
# else // APM2 Production Hardware (default)

3
ArduCopter/system.pde
View File

@ -107,7 +107,8 @@ static void init_ardupilot()
// Initialize Wire and SPI libraries
//
#ifndef DESKTOP_BUILD
Wire.begin();
I2c.begin();
I2c.timeOut(20);
#endif
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

19
ArduCopter/test.pde
View File

@ -903,14 +903,17 @@ test_mag(uint8_t argc, const Menu::arg *argv)
while(1){
delay(100);
compass.read();
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix());
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z);
} else {
Serial.println_P(PSTR("not healthy"));
}
if(Serial.available() > 0){
return (0);

19
ArduPlane/ArduPlane.pde
View File

@ -29,7 +29,7 @@ version 2.1 of the License, or (at your option) any later version.
#include <Arduino_Mega_ISR_Registry.h>
#include <APM_RC.h> // ArduPilot Mega RC Library
#include <AP_GPS.h> // ArduPilot GPS library
#include <Wire.h> // Arduino I2C lib
#include <I2C.h> // Wayne Truchsess I2C lib
#include <SPI.h> // Arduino SPI lib
#include <DataFlash.h> // ArduPilot Mega Flash Memory Library
#include <AP_ADC.h> // ArduPilot Mega Analog to Digital Converter Library
@ -621,11 +621,10 @@ static void medium_loop()
}
#if HIL_MODE != HIL_MODE_ATTITUDE
if(g.compass_enabled){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
if (g.compass_enabled && compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
#endif
/*{
Serial.print(dcm.roll_sensor, DEC); Serial.printf_P(PSTR("\t"));
@ -979,8 +978,12 @@ static void update_alt()
// this function is in place to potentially add a sonar sensor in the future
//altitude_sensor = BARO;
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100)
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
if (barometer.healthy) {
current_loc.alt = (1 - g.altitude_mix) * g_gps->altitude; // alt_MSL centimeters (meters * 100)
current_loc.alt += g.altitude_mix * (read_barometer() + home.alt);
} else if (g_gps->fix) {
current_loc.alt = g_gps->altitude; // alt_MSL centimeters (meters * 100)
}
#endif
geofence_check(true);

3
ArduPlane/geofence.pde
View File

@ -235,13 +235,12 @@ static void geofence_check(bool altitude_check_only)
}
// we are outside the fence
if (geofence_state->fence_triggered) {
if (geofence_state->fence_triggered && control_mode == GUIDED) {
// we have already triggered, don't trigger again until the
// user disables/re-enables using the fence channel switch
return;
}
// we are outside, and have not previously triggered.
geofence_state->fence_triggered = true;
geofence_state->breach_count++;

11
ArduPlane/sensors.pde
View File

@ -11,7 +11,7 @@ static void init_barometer(void)
long ground_pressure = 0;
int ground_temperature;
while(ground_pressure == 0){
while (ground_pressure == 0 || !barometer.healthy) {
barometer.read(); // Get initial data from absolute pressure sensor
ground_pressure = barometer.get_pressure();
ground_temperature = barometer.get_temperature();
@ -22,10 +22,12 @@ static void init_barometer(void)
for(int i = 0; i < 30; i++){ // We take some readings...
#if HIL_MODE == HIL_MODE_SENSORS
gcs_update(); // look for inbound hil packets
gcs_update(); // look for inbound hil packets
#endif
barometer.read(); // Get initial data from absolute pressure sensor
do {
barometer.read(); // Get pressure sensor
} while (!barometer.healthy);
ground_pressure = (ground_pressure * 9l + barometer.get_pressure()) / 10l;
ground_temperature = (ground_temperature * 9 + barometer.get_temperature()) / 10;
@ -59,7 +61,6 @@ static long read_barometer(void)
barometer.read(); // Get new data from absolute pressure sensor
//abs_pressure = (abs_pressure + barometer.get_pressure()) >> 1; // Small filtering
abs_pressure = ((float)abs_pressure * .7) + ((float)barometer.get_pressure() * .3); // large filtering
scaling = (float)g.ground_pressure / (float)abs_pressure;

3
ArduPlane/system.pde
View File

@ -107,7 +107,8 @@ static void init_ardupilot()
// Initialize Wire and SPI libraries
//
#ifndef DESKTOP_BUILD
Wire.begin();
I2c.begin();
I2c.timeOut(20);
#endif
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

46
ArduPlane/test.pde
View File

@ -547,9 +547,10 @@ test_imu(uint8_t argc, const Menu::arg *argv)
if(g.compass_enabled) {
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
medium_loopCounter = 0;
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
}
medium_loopCounter = 0;
}
}
@ -610,23 +611,28 @@ test_mag(uint8_t argc, const Menu::arg *argv)
medium_loopCounter++;
if(medium_loopCounter == 5){
compass.read(); // Read magnetometer
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
if (compass.read()) {
compass.calculate(dcm.get_dcm_matrix()); // Calculate heading
compass.null_offsets(dcm.get_dcm_matrix());
}
medium_loopCounter = 0;
}
counter++;
if (counter>20) {
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
if (compass.healthy) {
Vector3f maggy = compass.get_offsets();
Serial.printf_P(PSTR("Heading: %ld, XYZ: %d, %d, %d,\tXYZoff: %6.2f, %6.2f, %6.2f\n"),
(wrap_360(ToDeg(compass.heading) * 100)) /100,
compass.mag_x,
compass.mag_y,
compass.mag_z,
maggy.x,
maggy.y,
maggy.z);
} else {
Serial.println_P(PSTR("compass not healthy"));
}
counter=0;
}
}
@ -694,9 +700,13 @@ test_pressure(uint8_t argc, const Menu::arg *argv)
delay(100);
current_loc.alt = read_barometer() + home.alt;
Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
current_loc.alt / 100.0,
abs_pressure, 0.1*barometer.get_temperature());
if (!barometer.healthy) {
Serial.println_P(PSTR("not healthy"));
} else {
Serial.printf_P(PSTR("Alt: %0.2fm, Raw: %ld Temperature: %.1f\n"),
current_loc.alt / 100.0,
abs_pressure, 0.1*barometer.get_temperature());
}
if(Serial.available() > 0){
return (0);

20
libraries/AP_Baro/AP_Baro.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_H__
#define __AP_BARO_H__
@ -7,15 +8,16 @@
class AP_Baro
{
public:
AP_Baro() {}
virtual void init(AP_PeriodicProcess *scheduler)=0;
virtual uint8_t read() = 0;
virtual int32_t get_pressure() = 0;
virtual int16_t get_temperature() = 0;
virtual float get_altitude() = 0;
virtual int32_t get_raw_pressure() = 0;
virtual int32_t get_raw_temp() = 0;
bool healthy;
AP_Baro() {}
virtual bool init(AP_PeriodicProcess *scheduler)=0;
virtual uint8_t read() = 0;
virtual int32_t get_pressure() = 0;
virtual int16_t get_temperature() = 0;
virtual float get_altitude() = 0;
virtual int32_t get_raw_pressure() = 0;
virtual int32_t get_raw_temp() = 0;
};
#include "AP_Baro_MS5611.h"

112
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
APM_BMP085.cpp - Arduino Library for BMP085 absolute pressure sensor
Code by Jordi Mu<EFBFBD>oz and Jose Julio. DIYDrones.com
@ -41,7 +42,9 @@ extern "C" {
#include "WConstants.h"
}
#include <Wire.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <I2C.h>
#include "AP_Baro_BMP085.h"
#define BMP085_ADDRESS 0x77 //(0xEE >> 1)
@ -55,34 +58,19 @@ extern "C" {
// Public Methods //////////////////////////////////////////////////////////////
void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
{
byte buff[22];
int i = 0;
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
pinMode(BMP085_EOC, INPUT); // End Of Conversion (PC7) input
oss = 3; // Over Sampling setting 3 = High resolution
BMP085_State = 0; // Initial state
// We read the calibration data registers
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xAA);
if (Wire.endTransmission() != 0) {
// Error!
return;
}
Wire.requestFrom(BMP085_ADDRESS, 22);
//Wire.endTransmission();
while(Wire.available()){
buff[i] = Wire.receive(); // receive one byte
i++;
}
if (i != 22) {
// Error!
return;
// We read the calibration data registers
if (I2c.read(BMP085_ADDRESS, 0xAA, 22, buff) != 0) {
healthy = false;
return false;
}
ac1 = ((int)buff[0] << 8) | buff[1];
@ -97,10 +85,12 @@ void AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
mc = ((int)buff[18] << 8) | buff[19];
md = ((int)buff[20] << 8) | buff[21];
//Send a command to read Temp
//Send a command to read Temp
Command_ReadTemp();
BMP085_State = 1;
return;
healthy = true;
return true;
}
// Read the sensor. This is a state machine
@ -152,48 +142,27 @@ int32_t AP_Baro_BMP085::get_raw_temp() {
// Send command to Read Pressure
void AP_Baro_BMP085::Command_ReadPress()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x34+(oss << 6)); // write_register(0xF4, 0x34+(oversampling_setting << 6));
Wire.endTransmission();
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x34+(oss << 6)) != 0) {
healthy = false;
}
}
// Read Raw Pressure values
void AP_Baro_BMP085::ReadPress()
{
byte msb;
byte lsb;
byte xlsb;
uint8_t buf[3];
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
Wire.requestFrom(BMP085_ADDRESS, 3); // read a byte
while(!Wire.available()) {
// waiting
if (I2c.read(BMP085_ADDRESS, 0xF6, 3, buf) != 0) {
healthy = false;
return;
}
msb = Wire.receive();
while(!Wire.available()) {
// waiting
}
lsb = Wire.receive();
while(!Wire.available()) {
// waiting
}
xlsb = Wire.receive();
RawPress = (((long)msb << 16) | ((long)lsb << 8) | ((long)xlsb)) >> (8 - oss);
RawPress = (((long)buf[0] << 16) | ((long)buf[1] << 8) | ((long)buf[2])) >> (8 - oss);
if(_offset_press == 0){
_offset_press = RawPress;
RawPress = 0;
}else{
} else{
RawPress -= _offset_press;
}
// filter
@ -203,8 +172,9 @@ void AP_Baro_BMP085::ReadPress()
_press_index = 0;
RawPress = 0;
// sum our filter
for(uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
RawPress += _press_filter[i];
}
@ -217,35 +187,27 @@ void AP_Baro_BMP085::ReadPress()
// Send Command to Read Temperature
void AP_Baro_BMP085::Command_ReadTemp()
{
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF4);
Wire.send(0x2E);
Wire.endTransmission();
if (I2c.write(BMP085_ADDRESS, 0xF4, 0x2E) != 0) {
healthy = false;
}
}
// Read Raw Temperature values
void AP_Baro_BMP085::ReadTemp()
{
byte tmp;
Wire.beginTransmission(BMP085_ADDRESS);
Wire.send(0xF6);
Wire.endTransmission();
uint8_t buf[2];
Wire.beginTransmission(BMP085_ADDRESS);
Wire.requestFrom(BMP085_ADDRESS,2);
if (I2c.read(BMP085_ADDRESS, 0xF6, 2, buf) != 0) {
healthy = false;
return;
}
RawTemp = buf[0];
RawTemp = (RawTemp << 8) | buf[1];
while(!Wire.available()); // wait
RawTemp = Wire.receive();
while(!Wire.available()); // wait
tmp = Wire.receive();
RawTemp = RawTemp << 8 | tmp;
if(_offset_temp == 0){
if (_offset_temp == 0){
_offset_temp = RawTemp;
RawTemp = 0;
}else{
} else {
RawTemp -= _offset_temp;
}

3
libraries/AP_Baro/AP_Baro_BMP085.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_BMP085_H__
#define __AP_BARO_BMP085_H__
@ -16,7 +17,7 @@ class AP_Baro_BMP085 : public AP_Baro
/* AP_Baro public interface: */
void init(AP_PeriodicProcess * scheduler);
bool init(AP_PeriodicProcess * scheduler);
uint8_t read();
int32_t get_pressure();
int16_t get_temperature();

7
libraries/AP_Baro/AP_Baro_BMP085_hil.cpp
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
extern "C" {
// AVR LibC Includes
@ -14,9 +15,10 @@ AP_Baro_BMP085_HIL::AP_Baro_BMP085_HIL()
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler)
bool AP_Baro_BMP085_HIL::init(AP_PeriodicProcess * scheduler)
{
BMP085_State=1;
BMP085_State=1;
return true;
}
@ -46,6 +48,7 @@ void AP_Baro_BMP085_HIL::setHIL(float _Temp, float _Press)
// TODO: map floats to raw
Temp = _Temp;
Press = _Press;
healthy = true;
}
int32_t AP_Baro_BMP085_HIL::get_pressure() {

13
libraries/AP_Baro/AP_Baro_BMP085_hil.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_BMP085_HIL_H__
#define __AP_BARO_BMP085_HIL_H__
@ -6,16 +7,18 @@
class AP_Baro_BMP085_HIL
{
private:
uint8_t BMP085_State;
private:
uint8_t BMP085_State;
int16_t Temp;
int32_t Press;
public:
public:
AP_Baro_BMP085_HIL(); // Constructor
//int Altitude;
bool healthy;
uint8_t oss;
void init(AP_PeriodicProcess * scheduler);
bool init(AP_PeriodicProcess * scheduler);
uint8_t read();
int32_t get_pressure();
int16_t get_temperature();

20
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
/*
APM_MS5611.cpp - Arduino Library for MS5611-01BA01 absolute pressure sensor
Code by Jose Julio, Pat Hickey and Jordi Muñoz. DIYDrones.com
@ -110,12 +111,12 @@ void AP_Baro_MS5611::_spi_write(uint8_t reg)
// Public Methods //////////////////////////////////////////////////////////////
// SPI should be initialized externally
void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
{
pinMode(MS5611_CS, OUTPUT); // Chip select Pin
digitalWrite(MS5611_CS, HIGH);
delay(1);
digitalWrite(MS5611_CS, HIGH);
delay(1);
_spi_write(CMD_MS5611_RESET);
delay(4);
@ -128,14 +129,17 @@ void AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
C6 = _spi_read_16bits(CMD_MS5611_PROM_C6);
//Send a command to read Temp first
//Send a command to read Temp first
_spi_write(CMD_CONVERT_D2_OSR4096);
_timer = micros();
_state = 1;
Temp=0;
Press=0;
Temp=0;
Press=0;
scheduler->register_process( AP_Baro_MS5611::_update );
scheduler->register_process( AP_Baro_MS5611::_update );
healthy = true;
return true;
}

3
libraries/AP_Baro/AP_Baro_MS5611.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef __AP_BARO_MS5611_H__
#define __AP_BARO_MS5611_H__
@ -10,7 +11,7 @@ class AP_Baro_MS5611 : public AP_Baro
AP_Baro_MS5611() {} // Constructor
/* AP_Baro public interface: */
void init(AP_PeriodicProcess *scheduler);
bool init(AP_PeriodicProcess *scheduler);
uint8_t read();
int32_t get_pressure(); // in mbar*100 units
int16_t get_temperature(); // in celsius degrees * 100 units

56
libraries/AP_Baro/examples/AP_Baro_BMP085_test/AP_Baro_BMP085_test.pde
View File

@ -4,52 +4,74 @@
*/
#include <FastSerial.h>
#include <Wire.h>
#include <APM_BMP085.h> // ArduPilot Mega BMP085 Library
#include <I2C.h>
#include <SPI.h>
#include <AP_Baro.h> // ArduPilot Mega BMP085 Library
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_InertialSensor.h>
#include <AP_Math.h>
#include <AP_Common.h>
APM_BMP085_Class APM_BMP085;
#ifndef APM2_HARDWARE
# define APM2_HARDWARE 0
#endif
AP_Baro_BMP085 APM_BMP085(APM2_HARDWARE);
unsigned long timer;
FastSerialPort0(Serial);
#ifdef APM2_HARDWARE
static bool apm2_hardware = true;
#else
static bool apm2_hardware = false;
#endif
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
void setup()
{
Serial.begin(115200);
Serial.println("ArduPilot Mega BMP085 library test");
Serial.println("Initialising barometer..."); delay(100);
if (!APM_BMP085.Init(1, apm2_hardware)) {
I2c.begin();
I2c.timeOut(20);
//I2c.setSpeed(true);
isr_registry.init();
scheduler.init(&isr_registry);
if (!APM_BMP085.init(&scheduler)) {
Serial.println("Barometer initialisation FAILED\n");
}
Serial.println("initialisation complete."); delay(100);
delay(1000);
timer = millis();
timer = micros();
}
void loop()
{
int ch;
float tmp_float;
float Altitude;
if((millis()- timer) > 50){
timer = millis();
APM_BMP085.Read();
if((micros()- timer) > 50000L){
timer = micros();
APM_BMP085.read();
unsigned long read_time = micros() - timer;
if (!APM_BMP085.healthy) {
Serial.println("not healthy");
return;
}
Serial.print("Pressure:");
Serial.print(APM_BMP085.Press);
Serial.print(APM_BMP085.get_pressure());
Serial.print(" Temperature:");
Serial.print(APM_BMP085.Temp / 10.0);
Serial.print(APM_BMP085.get_temperature());
Serial.print(" Altitude:");
tmp_float = (APM_BMP085.Press / 101325.0);
tmp_float = (APM_BMP085.get_pressure() / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);
Serial.print(Altitude);
Serial.printf(" t=%u", (unsigned)read_time);
Serial.println();
}
}

86
libraries/AP_Baro/examples/AP_Baro_MS5611_test/AP_Baro_MS5611_test.pde
View File

@ -1,12 +1,21 @@
#include <stdint.h>
#include <AP_Common.h>
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <FastSerial.h>
#include <I2C.h>
#include <SPI.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
#include <AP_Baro.h> // ArduPilot Mega ADC Library
FastSerialPort0(Serial);
AP_Baro_MS5611 baro;
Arduino_Mega_ISR_Registry isr_registry;
AP_TimerProcess scheduler;
unsigned long timer;
void setup()
{
@ -15,54 +24,41 @@ void setup()
delay(1000);
pinMode(63, OUTPUT);
digitalWrite(63, HIGH);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later
isr_registry.init();
scheduler.init(&isr_registry);
baro.init();
pinMode(63, OUTPUT);
digitalWrite(63, HIGH);
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV32); // 500khz for debugging, increase later
baro.init(&scheduler);
timer = micros();
}
void loop()
{
int32_t pres;
int32_t temp;
Serial.println("Start Conversions");
baro._start_conversion_D1();
delay(10);
bool res1 = baro._adc_read(&pres);
baro._start_conversion_D2();
delay(10);
bool res2 = baro._adc_read(&temp);
if (res1) {
Serial.printf("Pressure raw value %ld\r\n",pres);
} else {
Serial.println("ADC conversion D1 unsuccessful");
}
if (res2) {
Serial.printf("Temp raw value %ld\r\n",pres);
} else {
Serial.println("ADC conversion D2 unsuccessful");
}
Serial.println("---");
delay(250);
}
void update_and_print()
{
int32_t pres;
float temp;
baro.update();
pres = baro.get_pressure();
temp = baro.get_temp();
Serial.printf("p: %ld t: %f \r\n", pres, temp);
delay(100);
float tmp_float;
float Altitude;
if((micros()- timer) > 50000L){
timer = micros();
baro.read();
unsigned long read_time = micros() - timer;
if (!baro.healthy) {
Serial.println("not healthy");
return;
}
Serial.print("Pressure:");
Serial.print(baro.get_pressure());
Serial.print(" Temperature:");
Serial.print(baro.get_temperature());
Serial.print(" Altitude:");
tmp_float = (baro.get_pressure() / 101325.0);
tmp_float = pow(tmp_float, 0.190295);
Altitude = 44330 * (1.0 - tmp_float);
Serial.print(Altitude);
Serial.printf(" t=%u", (unsigned)read_time);
Serial.println();
}
}

6
libraries/AP_Compass/AP_Compass_HIL.cpp
View File

@ -13,9 +13,10 @@
// Public Methods //////////////////////////////////////////////////////////////
void AP_Compass_HIL::read()
bool AP_Compass_HIL::read()
{
// values set by setHIL function
// values set by setHIL function
return true;
}
// Update raw magnetometer values from HIL data
@ -26,4 +27,5 @@ void AP_Compass_HIL::setHIL(float _mag_x, float _mag_y, float _mag_z)
mag_x = _mag_x;
mag_y = _mag_y;
mag_z = _mag_z;
healthy = true;
}

6
libraries/AP_Compass/AP_Compass_HIL.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_Compass_HIL_H
#define AP_Compass_HIL_H
@ -7,9 +8,8 @@ class AP_Compass_HIL : public Compass
{
public:
AP_Compass_HIL(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) { product_id = AP_COMPASS_TYPE_HIL; }
void read();
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
bool read(void);
void setHIL(float Mag_X, float Mag_Y, float Mag_Z);
};
#endif

107
libraries/AP_Compass/AP_Compass_HMC5843.cpp
View File

@ -16,8 +16,9 @@
// AVR LibC Includes
#include <math.h>
#include "WConstants.h"
#include <FastSerial.h>
#include <Wire.h>
#include <I2C.h>
#include "AP_Compass_HMC5843.h"
#define COMPASS_ADDRESS 0x1E
@ -47,40 +48,23 @@
#define DataOutputRate_75HZ 0x06
// read_register - read a register value
static bool
read_register(int address, byte *value)
bool AP_Compass_HMC5843::read_register(uint8_t address, uint8_t *value)
{
bool ret = false;
*value = 0;
Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(address); //sends address to read from
if (0 != Wire.endTransmission())
return false;
Wire.requestFrom(COMPASS_ADDRESS, 1); // request 1 byte from device
if( Wire.available() ) {
*value = Wire.receive(); // receive one byte
ret = true;
}
if (0 != Wire.endTransmission())
return false;
return ret;
if (I2c.read((uint8_t)COMPASS_ADDRESS, address, 1, value) != 0) {
healthy = false;
return false;
}
return true;
}
// write_register - update a register value
static bool
write_register(int address, byte value)
bool AP_Compass_HMC5843::write_register(uint8_t address, byte value)
{
Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(address);
Wire.send(value);
if (0 != Wire.endTransmission())
return false;
delay(10);
return true;
if (I2c.write((uint8_t)COMPASS_ADDRESS, address, value) != 0) {
healthy = false;
return false;
}
return true;
}
/*
@ -96,25 +80,10 @@ static void rotate_for_5883L(AP_VarS<Matrix3f> *_orientation_matrix)
// Read Sensor data
bool AP_Compass_HMC5843::read_raw()
{
int i = 0;
byte buff[6];
uint8_t buff[6];
Wire.beginTransmission(COMPASS_ADDRESS);
Wire.send(0x03); //sends address to read from
if (0 != Wire.endTransmission())
return false;
Wire.requestFrom(COMPASS_ADDRESS, 6); // request 6 bytes from device
while (Wire.available()) {
buff[i] = Wire.receive(); // receive one byte
i++;
}
if (0 != Wire.endTransmission())
return false;
if (i != 6) {
/* we didn't get enough bytes */
if (I2c.read(COMPASS_ADDRESS, 0x03, 6, buff) != 0) {
healthy = false;
return false;
}
@ -140,13 +109,26 @@ bool AP_Compass_HMC5843::read_raw()
}
/*
re-initialise after a IO error
*/
bool AP_Compass_HMC5843::re_initialise()
{
if (! write_register(ConfigRegA, _base_config) ||
! write_register(ConfigRegB, magGain) ||
! write_register(ModeRegister, ContinuousConversion))
return false;
return true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
int numAttempts = 0, good_count = 0;
bool success = false;
byte base_config; // used to test compass type
byte calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
@ -156,10 +138,11 @@ AP_Compass_HMC5843::init()
// determine if we are using 5843 or 5883L
if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
! read_register(ConfigRegA, &base_config)) {
! read_register(ConfigRegA, &_base_config)) {
healthy = false;
return false;
}
if ( base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
int old_product_id = product_id;
@ -175,7 +158,7 @@ AP_Compass_HMC5843::init()
*/
rotate_for_5883L(&_orientation_matrix);
}
} else if (base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
@ -253,23 +236,22 @@ AP_Compass_HMC5843::init()
}
// leave test mode
if (! write_register(ConfigRegA, base_config))
if (!re_initialise()) {
return false;
delay(50);
if (! write_register(ConfigRegB, magGain) ||
! write_register(ModeRegister, ContinuousConversion))
return false;
delay(50);
}
return success;
}
// Read Sensor data
void
AP_Compass_HMC5843::read()
bool AP_Compass_HMC5843::read()
{
if (!healthy && !re_initialise()) {
return false;
}
if (!read_raw()) {
return;
return false;
}
mag_x *= calibration[0];
@ -285,6 +267,9 @@ AP_Compass_HMC5843::read()
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
healthy = true;
return true;
}
// set orientation

12
libraries/AP_Compass/AP_Compass_HMC5843.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef AP_Compass_HMC5843_H
#define AP_Compass_HMC5843_H
@ -47,11 +48,16 @@ class AP_Compass_HMC5843 : public Compass
{
private:
float calibration[3];
virtual bool read_raw(void);
uint8_t _base_config;
virtual bool re_initialise(void);
bool read_register(uint8_t address, uint8_t *value);
bool write_register(uint8_t address, byte value);
public:
AP_Compass_HMC5843(AP_Var::Key key = AP_Var::k_key_none) : Compass(key) {}
virtual bool init();
virtual void read();
virtual bool read_raw();
virtual bool init(void);
virtual bool read(void);
virtual void set_orientation(const Matrix3f &rotation_matrix);
};

1
libraries/AP_Compass/Compass.cpp
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#include "Compass.h"
// Default constructor.

36
libraries/AP_Compass/Compass.h
View File

@ -1,3 +1,4 @@
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
#ifndef Compass_h
#define Compass_h
@ -40,6 +41,7 @@ public:
float heading_x; ///< compass vector X magnitude
float heading_y; ///< compass vector Y magnitude
unsigned long last_update; ///< millis() time of last update
bool healthy; ///< true if last read OK
/// Constructor
///
@ -56,7 +58,7 @@ public:
/// Read the compass and update the mag_ variables.
///
virtual void read() = 0;
virtual bool read(void) = 0;
/// Calculate the tilt-compensated heading_ variables.
///
@ -98,21 +100,21 @@ public:
///
virtual Vector3f &get_offsets();
/// Program new offset values.
///
/// XXX DEPRECATED
///
/// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value.
///
void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
/// Program new offset values.
///
/// XXX DEPRECATED
///
/// @param x Offset to the raw mag_x value.
/// @param y Offset to the raw mag_y value.
/// @param z Offset to the raw mag_z value.
///
void set_offsets(int x, int y, int z) { set_offsets(Vector3f(x, y, z)); }
/// Perform automatic offset updates using the results of the DCM matrix.
///
/// @param dcm_matrix The DCM result matrix.
///
void null_offsets(const Matrix3f &dcm_matrix);
/// Perform automatic offset updates using the results of the DCM matrix.
///
/// @param dcm_matrix The DCM result matrix.
///
void null_offsets(const Matrix3f &dcm_matrix);
/// Sets the local magnetic field declination.
@ -129,7 +131,7 @@ protected:
AP_Float _declination;
bool _null_init_done; ///< first-time-around flag used by offset nulling
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
Vector3f _mag_body_last; ///< ?? used by offset nulling
Matrix3f _last_dcm_matrix; ///< previous DCM matrix used by offset nulling
Vector3f _mag_body_last; ///< ?? used by offset nulling
};
#endif

47
libraries/AP_Compass/examples/AP_Compass_test/AP_Compass_test.pde
View File

@ -7,7 +7,7 @@
#include <AP_Common.h>
#include <AP_Compass.h> // Compass Library
#include <AP_Math.h> // ArduPilot Mega Vector/Matrix math Library
#include <Wire.h>
#include <I2C.h>
FastSerialPort0(Serial);
@ -23,7 +23,11 @@ void setup()
{
Serial.begin(115200);
Serial.println("Compass library test (HMC5843 and HMC5883L)");
Wire.begin();
I2c.begin();
I2c.timeOut(20);
// I2c.setSpeed(true);
if (!compass.init()) {
Serial.println("compass initialisation failed!");
while (1) ;
@ -50,17 +54,23 @@ void setup()
}
delay(3000);
timer = millis();
timer = micros();
}
void loop()
{
static float min[3], max[3], offset[3];
if((millis()- timer) > 100)
if((micros()- timer) > 100000L)
{
timer = millis();
timer = micros();
compass.read();
unsigned long read_time = micros() - timer;
if (!compass.healthy) {
Serial.println("not healthy");
return;
}
compass.calculate(0,0); // roll = 0, pitch = 0 for this example
// capture min
@ -85,25 +95,18 @@ void loop()
offset[2] = -(max[2]+min[2])/2;
// display all to user
Serial.print("Heading:");
Serial.print(ToDeg(compass.heading));
Serial.print(" (");
Serial.print(compass.mag_x);
Serial.print(",");
Serial.print(compass.mag_y);
Serial.print(",");
Serial.print(compass.mag_z);
Serial.print(")");
Serial.printf("Heading: %.2f (%3u,%3u,%3u) ",
ToDeg(compass.heading),
compass.mag_x,
compass.mag_y,
compass.mag_z);
// display offsets
Serial.print("\t offsets(");
Serial.print(offset[0]);
Serial.print(",");
Serial.print(offset[1]);
Serial.print(",");
Serial.print(offset[2]);
Serial.print(")");
Serial.printf("\t offsets(%.2f, %.2f, %.2f)",
offset[0], offset[1], offset[2]);
Serial.printf(" t=%u", (unsigned)read_time);
Serial.println();
}
}
}

1
libraries/AP_Compass/examples/AP_Compass_test/Makefile
View File

@ -1,2 +1 @@
BOARD = mega
include ../../../AP_Common/Arduino.mk

2
libraries/AP_DCM/AP_DCM.cpp
View File

@ -309,7 +309,7 @@ AP_DCM::drift_correction(void)
//*****YAW***************
if (_compass) {
if (_compass && _compass->healthy) {
// We make the gyro YAW drift correction based on compass magnetic heading
error_course = (_dcm_matrix.a.x * _compass->heading_y) - (_dcm_matrix.b.x * _compass->heading_x); // Equation 23, Calculating YAW error

2
libraries/AP_InertialSensor/examples/MPU6000/MPU6000.pde
View File

@ -5,7 +5,6 @@
//
#include <FastSerial.h>
#include <Wire.h>
#include <SPI.h>
#include <Arduino_Mega_ISR_Registry.h>
#include <AP_PeriodicProcess.h>
@ -24,7 +23,6 @@ void setup(void)
Serial.begin(115200);
Serial.println("Doing INS startup...");
Wire.begin();
SPI.begin();
SPI.setClockDivider(SPI_CLOCK_DIV16); // 1MHZ SPI rate

2
libraries/Desktop/Desktop.mk
View File

@ -185,7 +185,7 @@ else
endif
# these are library objects we don't want in the desktop build (maybe we'll add them later)
NODESKTOP := DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp
NODESKTOP := I2C/I2C.cpp DataFlash/DataFlash_APM1.cpp FastSerial/FastSerial.cpp AP_Compass/AP_Compass_HMC5843.cpp AP_Baro/AP_Baro_BMP085.cpp
#
# Find sketchbook libraries referenced by the sketch.

557
libraries/I2C/I2C.cpp Normal file
View File

@ -0,0 +1,557 @@
/*
I2C.cpp - I2C library
Copyright (c) 2011 Wayne Truchsess. All right reserved.
Rev 2.0 - September 19th, 2011
- Added support for timeout function to prevent
and recover from bus lockup (thanks to PaulS
and CrossRoads on the Arduino forum)
- Changed return type for stop() from void to
uint8_t to handle timeOut function
Rev 1.0 - August 8th, 2011
This is a modified version of the Arduino Wire/TWI
library. Functions were rewritten to provide more functionality
and also the use of Repeated Start. Some I2C devices will not
function correctly without the use of a Repeated Start. The
initial version of this library only supports the Master.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "WProgram.h"
#include <inttypes.h>
#include "I2C.h"
uint8_t I2C::bytesAvailable = 0;
uint8_t I2C::bufferIndex = 0;
uint8_t I2C::totalBytes = 0;
uint16_t I2C::timeOutDelay = 0;
I2C::I2C()
{
}
////////////// Public Methods ////////////////////////////////////////
void I2C::begin()
{
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
// activate internal pull-ups for twi
// as per note from atmega8 manual pg167
sbi(PORTC, 4);
sbi(PORTC, 5);
#else
// activate internal pull-ups for twi
// as per note from atmega128 manual pg204
sbi(PORTD, 0);
sbi(PORTD, 1);
#endif
// initialize twi prescaler and bit rate
cbi(TWSR, TWPS0);
cbi(TWSR, TWPS1);
TWBR = ((CPU_FREQ / 100000) - 16) / 2;
// enable twi module, acks, and twi interrupt
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA);
}
void I2C::end()
{
TWCR = 0;
}
void I2C::timeOut(uint16_t _timeOut)
{
timeOutDelay = _timeOut;
}
void I2C::setSpeed(boolean _fast)
{
if(!_fast)
{
TWBR = ((CPU_FREQ / 100000) - 16) / 2;
}
else
{
TWBR = ((CPU_FREQ / 400000) - 16) / 2;
}
}
void I2C::pullup(boolean activate)
{
if(activate)
{
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
// activate internal pull-ups for twi
// as per note from atmega8 manual pg167
sbi(PORTC, 4);
sbi(PORTC, 5);
#else
// activate internal pull-ups for twi
// as per note from atmega128 manual pg204
sbi(PORTD, 0);
sbi(PORTD, 1);
#endif
}
else
{
#if defined(__AVR_ATmega168__) || defined(__AVR_ATmega8__) || defined(__AVR_ATmega328P__)
// deactivate internal pull-ups for twi
// as per note from atmega8 manual pg167
cbi(PORTC, 4);
cbi(PORTC, 5);
#else
// deactivate internal pull-ups for twi
// as per note from atmega128 manual pg204
cbi(PORTD, 0);
cbi(PORTD, 1);
#endif
}
}
/////////////carry over from Wire library ///////////
uint8_t I2C::beginTransmission(uint8_t address)
{
returnStatusWire = 0;
returnStatus = 0;
returnStatus = start();
returnStatusWire = returnStatus;
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_W(address));
returnStatusWire = returnStatus;
return(returnStatus);
}
uint8_t I2C::beginTransmission(int address)
{
return(beginTransmission((uint8_t) address));
}
uint8_t I2C::send(uint8_t databyte)
{
if(returnStatusWire)
{
return(returnStatusWire);
}
returnStatus = 0;
returnStatus = sendByte(databyte);
returnStatusWire = returnStatus;
return(returnStatus);
}
uint8_t I2C::send(int databyte)
{
return(send((uint8_t) databyte));
}
uint8_t I2C::endTransmission()
{
stop();
return(returnStatusWire);
}
uint8_t I2C::requestFrom(int address, int numberBytes)
{
return(requestFrom((uint8_t) address, (uint8_t) numberBytes));
}
uint8_t I2C::requestFrom(uint8_t address, uint8_t numberBytes)
{
returnStatus = 0;
returnStatus = read(address,numberBytes);
if(!returnStatus)
{
return(numberBytes);
}
return(0);
}
uint8_t I2C::available()
{
return(bytesAvailable);
}
uint8_t I2C::receive()
{
bufferIndex = totalBytes - bytesAvailable;
if(!bytesAvailable)
{
bufferIndex = 0;
return(0);
}
bytesAvailable--;
return(data[bufferIndex]);
}
/////////////////////////////////////////////////////
uint8_t I2C::write(uint8_t address, uint8_t registerAddress)
{
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_W(address));
if(returnStatus){return(returnStatus);}
returnStatus = sendByte(registerAddress);
if(returnStatus){return(returnStatus);}
returnStatus = stop();
return(returnStatus);
}
uint8_t I2C::write(int address, int registerAddress)
{
return(write((uint8_t) address, (uint8_t) registerAddress));
}
uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t databyte)
{
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_W(address));
if(returnStatus){return(returnStatus);}
returnStatus = sendByte(registerAddress);
if(returnStatus){return(returnStatus);}
returnStatus = sendByte(databyte);
if(returnStatus){return(returnStatus);}
returnStatus = stop();
return(returnStatus);
}
uint8_t I2C::write(int address, int registerAddress, int databyte)
{
return(write((uint8_t) address, (uint8_t) registerAddress, (uint8_t) databyte));
}
uint8_t I2C::write(uint8_t address, uint8_t registerAddress, char *databytes)
{
uint8_t bufferLength = strlen(databytes);
returnStatus = 0;
returnStatus = write(address, registerAddress, (uint8_t*)databytes, bufferLength);
return(returnStatus);
}
uint8_t I2C::write(uint8_t address, uint8_t registerAddress, uint8_t *databytes, uint8_t numberBytes)
{
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_W(address));
if(returnStatus){return(returnStatus);}
returnStatus = sendByte(registerAddress);
if(returnStatus){return(returnStatus);}
for (uint8_t i = 0; i < numberBytes; i++)
{
returnStatus = sendByte(databytes[i]);
if(returnStatus){return(returnStatus);}
}
returnStatus = stop();
return(returnStatus);
}
uint8_t I2C::read(int address, int numberBytes)
{
return(read((uint8_t) address, (uint8_t) numberBytes));
}
uint8_t I2C::read(uint8_t address, uint8_t numberBytes)
{
bytesAvailable = 0;
bufferIndex = 0;
if(numberBytes == 0){numberBytes++;}
nack = numberBytes - 1;
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_R(address));
if(returnStatus){return(returnStatus);}
for(uint8_t i = 0; i < numberBytes; i++)
{
if( i == nack )
{
returnStatus = receiveByte(0);
if(returnStatus != MR_DATA_NACK){return(returnStatus);}
}
else
{
returnStatus = receiveByte(1);
if(returnStatus != MR_DATA_ACK){return(returnStatus);}
}
data[i] = TWDR;
bytesAvailable = i+1;
totalBytes = i+1;
}
returnStatus = stop();
return(returnStatus);
}
uint8_t I2C::read(int address, int registerAddress, int numberBytes)
{
return(read((uint8_t) address, (uint8_t) registerAddress, (uint8_t) numberBytes));
}
uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes)
{
bytesAvailable = 0;
bufferIndex = 0;
if(numberBytes == 0){numberBytes++;}
nack = numberBytes - 1;
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_W(address));
if(returnStatus){return(returnStatus);}
returnStatus = sendByte(registerAddress);
if(returnStatus){return(returnStatus);}
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_R(address));
if(returnStatus){return(returnStatus);}
for(uint8_t i = 0; i < numberBytes; i++)
{
if( i == nack )
{
returnStatus = receiveByte(0);
if(returnStatus != MR_DATA_NACK){return(returnStatus);}
}
else
{
returnStatus = receiveByte(1);
if(returnStatus != MR_DATA_ACK){return(returnStatus);}
}
data[i] = TWDR;
bytesAvailable = i+1;
totalBytes = i+1;
}
returnStatus = stop();
return(returnStatus);
}
uint8_t I2C::read(uint8_t address, uint8_t numberBytes, uint8_t *dataBuffer)
{
bytesAvailable = 0;
bufferIndex = 0;
if(numberBytes == 0){numberBytes++;}
nack = numberBytes - 1;
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_R(address));
if(returnStatus){return(returnStatus);}
for(uint8_t i = 0; i < numberBytes; i++)
{
if( i == nack )
{
returnStatus = receiveByte(0);
if(returnStatus != MR_DATA_NACK){return(returnStatus);}
}
else
{
returnStatus = receiveByte(1);
if(returnStatus != MR_DATA_ACK){return(returnStatus);}
}
dataBuffer[i] = TWDR;
bytesAvailable = i+1;
totalBytes = i+1;
}
returnStatus = stop();
return(returnStatus);
}
uint8_t I2C::read(uint8_t address, uint8_t registerAddress, uint8_t numberBytes, uint8_t *dataBuffer)
{
bytesAvailable = 0;
bufferIndex = 0;
if(numberBytes == 0){numberBytes++;}
nack = numberBytes - 1;
returnStatus = 0;
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_W(address));
if(returnStatus){return(returnStatus);}
returnStatus = sendByte(registerAddress);
if(returnStatus){return(returnStatus);}
returnStatus = start();
if(returnStatus){return(returnStatus);}
returnStatus = sendAddress(SLA_R(address));
if(returnStatus){return(returnStatus);}
for(uint8_t i = 0; i < numberBytes; i++)
{
if( i == nack )
{
returnStatus = receiveByte(0);
if(returnStatus != MR_DATA_NACK){return(returnStatus);}
}
else
{
returnStatus = receiveByte(1);
if(returnStatus != MR_DATA_ACK){return(returnStatus);}
}
dataBuffer[i] = TWDR;
bytesAvailable = i+1;
totalBytes = i+1;
}
returnStatus = stop();
return(returnStatus);
}
/////////////// Private Methods ////////////////////////////////////////
uint8_t I2C::start()
{
unsigned long startingTime = millis();
TWCR = (1<<TWINT)|(1<<TWSTA)|(1<<TWEN);
while (!(TWCR & (1<<TWINT)))
{
if(!timeOutDelay){continue;}
if((millis() - startingTime) >= timeOutDelay)
{
lockUp();
return(1);
}
}
if ((TWI_STATUS == START) || (TWI_STATUS == REPEATED_START))
{
return(0);
}
return(TWI_STATUS);
}
uint8_t I2C::sendAddress(uint8_t i2cAddress)
{
TWDR = i2cAddress;
unsigned long startingTime = millis();
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)))
{
if(!timeOutDelay){continue;}
if((millis() - startingTime) >= timeOutDelay)
{
lockUp();
return(1);
}
}
if ((TWI_STATUS == MT_SLA_ACK) || (TWI_STATUS == MR_SLA_ACK))
{
return(0);
}
return(TWI_STATUS);
}
uint8_t I2C::sendByte(uint8_t i2cData)
{
TWDR = i2cData;
unsigned long startingTime = millis();
TWCR = (1<<TWINT) | (1<<TWEN);
while (!(TWCR & (1<<TWINT)))
{
if(!timeOutDelay){continue;}
if((millis() - startingTime) >= timeOutDelay)
{
lockUp();
return(1);
}
}
if (TWI_STATUS == MT_DATA_ACK)
{
return(0);
}
return(TWI_STATUS);
}
uint8_t I2C::receiveByte(boolean ack)
{
unsigned long startingTime = millis();
if(ack)
{
TWCR = (1<<TWINT) | (1<<TWEN) | (1<<TWEA);
}
else
{
TWCR = (1<<TWINT) | (1<<TWEN);
}
while (!(TWCR & (1<<TWINT)))
{
if(!timeOutDelay){continue;}
if((millis() - startingTime) >= timeOutDelay)
{
lockUp();
return(1);
}
}
return(TWI_STATUS);
}
uint8_t I2C::stop()
{
unsigned long startingTime = millis();
TWCR = (1<<TWINT)|(1<<TWEN)| (1<<TWSTO);
while ((TWCR & (1<<TWSTO)))
{
if(!timeOutDelay){continue;}
if((millis() - startingTime) >= timeOutDelay)
{
lockUp();
return(1);
}
}
return(0);
}
void I2C::lockUp()
{
TWCR = 0; //releases SDA and SCL lines to high impedance
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); //reinitialize TWI
}
SIGNAL(TWI_vect)
{
switch(TWI_STATUS){
case 0x20:
case 0x30:
case 0x48:
TWCR = (1<<TWINT)|(1<<TWEN)| (1<<TWSTO); // send a stop
break;
case 0x38:
case 0x68:
case 0x78:
case 0xB0:
TWCR = 0; //releases SDA and SCL lines to high impedance
TWCR = _BV(TWEN) | _BV(TWIE) | _BV(TWEA); //reinitialize TWI
break;
}
}
I2C I2c = I2C();

116
libraries/I2C/I2C.h Normal file
View File

@ -0,0 +1,116 @@
/*
I2C.h - I2C library
Copyright (c) 2011 Wayne Truchsess. All right reserved.
Rev 2.0 - September 19th, 2011
- Added support for timeout function to prevent
and recover from bus lockup (thanks to PaulS
and CrossRoads on the Arduino forum)
- Changed return type for stop() from void to
uint8_t to handle timeOut function
Rev 1.0 - August 8th, 2011
This is a modified version of the Arduino Wire/TWI
library. Functions were rewritten to provide more functionality
and also the use of Repeated Start. Some I2C devices will not
function correctly without the use of a Repeated Start. The
initial version of this library only supports the Master.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
#include "WProgram.h"
#include <inttypes.h>
#ifndef I2C_h
#define I2C_h
#define START 0x08
#define REPEATED_START 0x10
#define MT_SLA_ACK 0x18
#define MT_DATA_ACK 0x28
#define MR_SLA_ACK 0x40
#define MR_DATA_ACK 0x50
#define MR_DATA_NACK 0x58
#define TWI_STATUS (TWSR & 0xF8)
#define SLA_W(address) (address << 1)
#define SLA_R(address) ((address << 1) + 0x01)
#define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
#define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
#define MAX_BUFFER_SIZE 32
#ifndef CPU_FREQ
#define CPU_FREQ 16000000L
#endif
class I2C
{
public:
I2C();
void begin();
void end();
void timeOut(uint16_t);
void setSpeed(boolean);
void pullup(boolean);
///////carry over from Wire library////////
uint8_t returnStatusWire;
uint8_t beginTransmission(uint8_t);
uint8_t beginTransmission(int);
uint8_t send(uint8_t);
uint8_t send(int);
uint8_t endTransmission();
uint8_t requestFrom(uint8_t, uint8_t);
uint8_t requestFrom(int, int);
uint8_t available();
///////////////////////////////////////////
uint8_t write(uint8_t, uint8_t);
uint8_t write(int, int);
uint8_t write(uint8_t, uint8_t, uint8_t);
uint8_t write(int, int, int);
uint8_t write(uint8_t, uint8_t, char*);
uint8_t write(uint8_t, uint8_t, uint8_t*, uint8_t);
uint8_t read(uint8_t, uint8_t);
uint8_t read(int, int);
uint8_t read(uint8_t, uint8_t, uint8_t);
uint8_t read(int, int, int);
uint8_t read(uint8_t, uint8_t, uint8_t*);
uint8_t read(uint8_t, uint8_t, uint8_t, uint8_t*);
uint8_t receive();
private:
uint8_t start();
uint8_t sendAddress(uint8_t);
uint8_t sendByte(uint8_t);
uint8_t receiveByte(boolean);
uint8_t stop();
void lockUp();
uint8_t returnStatus;
uint8_t nack;
uint8_t data[MAX_BUFFER_SIZE];
static uint8_t bytesAvailable;
static uint8_t bufferIndex;
static uint8_t totalBytes;
static uint16_t timeOutDelay;
};
extern I2C I2c;
#endif

70
libraries/I2C/examples/HMC5883L/HMC5883L.pde Normal file
View File

@ -0,0 +1,70 @@
/*******************************************
Sample sketch that configures an HMC5883L 3 axis
magnetometer to continuous mode and reads back
the three axis of data.
Code compiles to a size of 1500 bytes
Equivalent Wire Library code compiles to 2032 bytes
*******************************************/
#include <I2C.h>
#define HMC5883L 0x1E
int x = 0;
int y = 0;
int z = 0;
void setup()
{
I2c.begin();
I2c.write(HMC5883L,0x02,0x00); //configure device for continuous mode
}
void loop()
{
I2c.read(HMC5883L,0x03,6); //read 6 bytes (x,y,z) from the device
x = I2c.receive() << 8;
x |= I2c.receive();
y = I2c.receive() << 8;
y |= I2c.receive();
z = I2c.receive() << 8;
z |= I2c.receive();
}
/* Wire library equivalent would be this
//#include <Wire.h>
#define HMC5883L 0x1E
int x = 0;
int y = 0;
int z = 0;
void setup()
{
Wire.begin();
Wire.beginTransmission(HMC5883L);
Wire.send(0x02);
Wire.send(0x00);
Wire.endTransmission();
}
void loop()
{
Wire.beginTransmission(HMC5883L);
Wire.send(0x03);
Wire.endTransmission();
Wire.requestFrom(HMC5883L,6);
x = Wire.receive() << 8;
x |= Wire.receive();
y = Wire.receive() << 8;
y |= Wire.receive();
z = Wire.receive() << 8;
z |= Wire.receive();
}
********************************************/

36
libraries/I2C/keywords.txt Normal file
View File

@ -0,0 +1,36 @@
#######################################
# Syntax Coloring Map For I2C
#######################################
#######################################
# Datatypes (KEYWORD1)
#######################################
#######################################
# Methods and Functions (KEYWORD2)
#######################################
begin KEYWORD2
end KEYWORD2
timeOut KEYWORD2
setSpeed KEYWORD2
pullup KEYWORD2
write KEYWORD2
read KEYWORD2
beginTransmission KEYWORD2
send KEYWORD2
endTransmission KEYWORD2
requestFrom KEYWORD2
available KEYWORD2
receive KEYWORD2
#######################################
# Instances (KEYWORD2)
#######################################
I2c KEYWORD2
#######################################
# Constants (LITERAL1)
#######################################