Comment by Kevin Bloch on November 22, 2010 at 6:50pm

I would like to attach the first version of the TWI code - how do you add a .zip file to a blog entry?

Comment by Kevin Bloch on November 22, 2010 at 7:05pm

ok I'll attach as text the main and TWI routines for the IMU... I will attach the library files tonight / tomorrow. I've ended up hacking the Ardupilot so I'll need to get the release version and add the TWI functions as an option, I'll do this in the next few days and upload the code.

***Main Function***

// Released under Creative Commons License
// Code by Jordi Munoz and William Premerlani, Supported by Chris Anderson and Doug Weibel
// Version 1.0 for flat board updated by Doug Weibel and Jose Julio
// Version 1.7 includes support for SCP1000 absolute pressure sensor
// Version 1.8 uses DIYDrones GPS, FastSerial, and Compass libraries

// Axis definition: X axis pointing forward, Y axis pointing to the right and Z axis pointing down.
// Positive pitch : nose up
// Positive roll : right wing down
// Positive yaw : clockwise

//---Added for TWI---
#define TWI_FREQ 400000L
//-------------------
#include
#include
#include // ArduPilot Fast Serial Library
#include // ArduPilot GPS library
#include // ArduPilot Mega Magnetometer Library
//---Added for TWI---
#include
//-------------------
//**********************************************************************
// This section contains USER PARAMETERS !!!
//
//**********************************************************************

// *** NOTE! Hardware version - Can be used for v1 (daughterboards) or v2 (flat)
#define BOARD_VERSION 2 // 1 For V1 and 2 for V2

#define GPS_CONNECTION 0 // 0 for GPS pins, 1 for programming pins

// GPS Type Selection - Note Ublox or MediaTek is recommended. Support for NMEA is limited.
#define GPS_PROTOCOL 4 // 1 - NMEA, 2 - EM406, 3 - Ublox, 4 -- MediaTek

// Enable Air Start uses Remove Before Fly flag - connection to pin 6 on ArduPilot
#define ENABLE_AIR_START 0 // 1 if using airstart/groundstart signaling, 0 if not
#define GROUNDSTART_PIN 8 // Pin number used for ground start signal (recommend 10 on v1 and 8 on v2 hardware)

/*Min Speed Filter for Yaw drift Correction*/
#define SPEEDFILT 2 // >1 use min speed filter for yaw drift cancellation (m/s), 0=do not use speed filter

/*For debugging propurses*/
#define PRINT_DEBUG 0 //Will print Debug messages

//OUTPUTMODE=1 will print the corrected data, 0 will print uncorrected data of the gyros (with drift), 2 will print accelerometer only data
#define OUTPUTMODE 1

#define PRINT_DCM 0 //Will print the whole direction cosine matrix
#define PRINT_ANALOGS 0 //Will print the analog raw data
#define PRINT_EULER 0 //Will print the Euler angles Roll, Pitch and Yaw
#define PRINT_GPS 0 //Will print GPS data

// *** NOTE! To use ArduIMU with ArduPilot you must select binary output messages (change to 1 here)
#define PRINT_BINARY 1 //Will print binary message and suppress ASCII messages (above)

// *** NOTE! Performance reporting is only supported for Ublox. Set to 0 for others
#define PERFORMANCE_REPORTING 0 //Will include performance reports in the binary output ~ 1/2 min

/* Support for optional magnetometer (1 enabled, 0 dissabled) */
#define USE_MAGNETOMETER 0 // use 1 if you want to make yaw gyro drift corrections using the optional magnetometer

// Local magnetic declination
// I use this web : http://www.ngdc.noaa.gov/geomagmodels/Declination.jsp
#define MAGNETIC_DECLINATION 1.37 // corrects magnetic bearing to true north

/* Support for optional barometer (1 enabled, 0 dissabled) */
#define USE_BAROMETER 0 // use 1 if you want to get altitude using the optional absolute pressure sensor
#define ALT_MIX 50 // For binary messages: GPS or barometric altitude. 0 to 100 = % of barometric. For example 75 gives 25% GPS alt and 75% baro

#define TWI_MODE 1 //when enabled, send data via TWI
//**********************************************************************
// End of user parameters
//**********************************************************************

#define SOFTWARE_VER "1.8"

// GPS Selection
FastSerialPort0(Serial); // Instantiate the fast serial driver
#if GPS_PROTOCOL == 1
AP_GPS_NMEA GPS(&Serial);
#elif GPS_PROTOCOL == 2
AP_GPS_406 GPS(&Serial);
#elif GPS_PROTOCOL == 3
AP_GPS_UBLOX GPS(&Serial);
#elif GPS_PROTOCOL == 4
AP_GPS_MTK GPS(&Serial);
#else
# error Must define GPS_PROTOCOL with a valid value.
#endif

// ADC : Voltage reference 3.3v / 10bits(1024 steps) => 3.22mV/ADC step
// ADXL335 Sensitivity(from datasheet) => 330mV/g, 3.22mV/ADC step => 330/3.22 = 102.48
// Tested value : 101
#define GRAVITY 101 //this equivalent to 1G in the raw data coming from the accelerometer
#define Accel_Scale(x) x*(GRAVITY/9.81)//Scaling the raw data of the accel to actual acceleration in meters for seconds square

#define ToRad(x) (x*0.01745329252) // *pi/180
#define ToDeg(x) (x*57.2957795131) // *180/pi

// LPR530 & LY530 Sensitivity (from datasheet) => 3.33mV/º/s, 3.22mV/ADC step => 1.03
// Tested values : 0.96,0.96,0.94
#define Gyro_Gain_X 0.92 //X axis Gyro gain
#define Gyro_Gain_Y 0.92 //Y axis Gyro gain
#define Gyro_Gain_Z 0.94 //Z axis Gyro gain
#define Gyro_Scaled_X(x) x*ToRad(Gyro_Gain_X) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Y(x) x*ToRad(Gyro_Gain_Y) //Return the scaled ADC raw data of the gyro in radians for second
#define Gyro_Scaled_Z(x) x*ToRad(Gyro_Gain_Z) //Return the scaled ADC raw data of the gyro in radians for second

#define Kp_ROLLPITCH 0.015
#define Ki_ROLLPITCH 0.000010
#define Kp_YAW 1.2
//#define Kp_YAW 2.5 //High yaw drift correction gain - use with caution!
#define Ki_YAW 0.00005

#define ADC_WARM_CYCLES 75

#define FALSE 0
#define TRUE 1

//------Added for TWI------
#define ARDUIMU_ADDR 1 //my address
#define ARDUPILOT_ADDR 2 //the ardupilot address
#define IPC_CONNECT 2

uint8_t newMessage;
//-----------------------

float G_Dt=0.02; // Integration time (DCM algorithm)

long timeNow=0; // Hold the milliseond value for now
long timer=0; //general purpuse timer
long timer_old;
long timer24=0; //Second timer used to print values
boolean groundstartDone = false; // Used to not repeat ground start

float AN[8]; //array that store the 6 ADC filtered data
float AN_OFFSET[8]; //Array that stores the Offset of the gyros

float Accel_Vector[3]= {0,0,0}; //Store the acceleration in a vector
float Gyro_Vector[3]= {0,0,0};//Store the gyros rutn rate in a vector
float Omega_Vector[3]= {0,0,0}; //Corrected Gyro_Vector data
float Omega_P[3]= {0,0,0};//Omega Proportional correction
float Omega_I[3]= {0,0,0};//Omega Integrator
float Omega[3]= {0,0,0};

// Euler angles
float roll;
float pitch;
float yaw;

int toggleMode=0;

float errorRollPitch[3]= {0,0,0};
float errorYaw[3]= {0,0,0};
float errorCourse=180;
float COGX=0; //Course overground X axis
float COGY=1; //Course overground Y axis

unsigned int cycleCount=0;
byte gyro_sat=0;

float DCM_Matrix[3][3]= {
{
1,0,0 }
,{
0,1,0 }
,{
0,0,1 }
};
float Update_Matrix[3][3]={{0,1,2},{3,4,5},{6,7,8}}; //Gyros here

float Temporary_Matrix[3][3]={
{
0,0,0 }
,{
0,0,0 }
,{
0,0,0 }
};

// Startup GPS variables
int gps_fix_count = 5; //used to count 5 good fixes at ground startup

//ADC variables
volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference = DEFAULT;
volatile uint16_t analog_buffer[8];
volatile uint8_t analog_count[8];


#if BOARD_VERSION == 1
uint8_t sensors[6] = {0,2,1,3,5,4}; // Use these two lines for Hardware v1 (w/ daughterboards)
int SENSOR_SIGN[]= {1,-1,1,-1,1,-1,-1,-1,-1}; //Sensor: GYROX, GYROY, GYROZ, ACCELX, ACCELY, ACCELZ
#endif

#if BOARD_VERSION == 2
uint8_t sensors[6] = {6,7,3,0,1,2}; // For Hardware v2 flat
int SENSOR_SIGN[] = {1,-1,-1,1,-1,1,-1,-1,-1};
#endif

// Performance Monitoring variables
// Data collected and reported for ~1/2 minute intervals
#if PERFORMANCE_REPORTING == 1
int mainLoop_count = 0; //Main loop cycles since last report
int G_Dt_max = 0.0; //Max main loop cycle time in milliseconds
byte gyro_sat_count = 0;
byte adc_constraints = 0;
byte renorm_sqrt_count = 0;
byte renorm_blowup_count = 0;
byte gps_messages_sent = 0;
long perf_mon_timer = 0;
#endif
unsigned int imu_health = 65012;

boolean GPS_light = false;
boolean SATGYRO_light = false;
boolean YAWDRIFT_light = false;

//---Added for TWI---
int gps_sync_count = 0;
int gps_nosync = 0;
int IMU_count = 0;
//--------------------

//**********************************************************************
// This section contains SCP1000_D11 PARAMETERS !!!
//**********************************************************************
#if USE_BAROMETER == 1
#define SCP_MODE (9) // 9 = high speed mode, 10 = high resolution mode
#define PRESSURE_ADDR (0x11U) // IIC address of the SCP1000
// ************ #define START_ALTITUDE (217U) // default initial altitude in m above sea level

// When we have to manage data transfers via IIC directly we need to use the following addresses
fs// IIC address of the SCP1000 device forms the Top 7 bits of the address with the R/W bit as the LSB
#define READ_PRESSURE_ADDR (PRESSURE_ADDR1 | 1)
#define WRITE_PRESSURE_ADDR (PRESSURE_ADDR1)

// SCP1000 Register addresses
#define SNS_ADDR_POPERATION (0x03U) // OPERATON register
#define SNS_ADDR_PSTATUS (0x07U) // STATUS register
#define SNS_ADDR_PPRESSURE (0x80U) // DATARD16 Register (pressure)
#define SNS_ADDR_DATARD8 (0x7FU) // DAYARD8 Register
#define SNS_ADDR_PTEMP (0x81U) // TEMPOUT Register (temperature)

#ifndef TRUE
#define TRUE (0x01)
#endif
#ifndef FALSE
#define FALSE (0x00)
#endif

int temp_unfilt = 0;
int temperature = 0;
unsigned long press = 0;
unsigned long press_filt = 0;
unsigned long press_gnd = 0;
long ground_alt = 0; // Ground altitude in centimeters
long press_alt = 0; // Pressure altitude in centimeters

#endif

//*****************************************************************************************
void setup()
{
Serial.begin(38400, 128, 16);
//---Added for TWI---
#if TWI_MODE
Wire.begin(); // join i2c bus
#endif

//-------------------

pinMode(2,OUTPUT); //Serial Mux
if (GPS_CONNECTION == 0){
digitalWrite(2,HIGH); //Serial Mux
} else {
digitalWrite(2,LOW); //Serial Mux
}

pinMode(5,OUTPUT); //Red LED
pinMode(6,OUTPUT); // Blue LED
pinMode(7,OUTPUT); // Yellow LED
pinMode(GROUNDSTART_PIN,INPUT); // Remove Before Fly flag (pin 6 on ArduPilot)
digitalWrite(GROUNDSTART_PIN,HIGH);

digitalWrite(5,HIGH);
delay(100);
digitalWrite(6,HIGH);
delay(100);
digitalWrite(7,HIGH);
delay(100);
digitalWrite(5,LOW);
delay(100);
digitalWrite(6,LOW);
delay(100);
digitalWrite(7,LOW);

Analog_Reference(EXTERNAL);//Using external analog reference
Analog_Init();

debug_print("Welcome...");

#if BOARD_VERSION == 1
debug_print("You are using Hardware Version 1...");
#endif

#if BOARD_VERSION == 2
debug_print("You are using Hardware Version 2...");
#endif

GPS.init(); // GPS Initialization

debug_handler(0); //Printing version

#if USE_MAGNETOMETER == 1
APM_Compass.Init(); // I2C initialization
debug_handler(3);
#endif

// SETUP FOR SCP1000_D11
#if USE_BAROMETER==1
debug_handler(4);
setup_scp();
#endif

if(ENABLE_AIR_START){
debug_handler(1);
startup_air();
}else{
debug_handler(2);
startup_ground();
}


delay(250);

Read_adc_raw(); // ADC initialization
timer=DIYmillis();
delay(20);

}

//***************************************************************************************
void loop() //Main Loop
{
timeNow = millis();

if((timeNow-timer)>=20) // Main loop runs at 50Hz
{
timer_old = timer;
timer = timeNow;

#if PERFORMANCE_REPORTING == 1
mainLoop_count++;
if (timer-timer_old > G_Dt_max) G_Dt_max = timer-timer_old;
#endif

G_Dt = (timer-timer_old)/1000.0; // Real time of loop run. We use this on the DCM algorithm (gyro integration time)
if(G_Dt > 1)
G_Dt = 0; //Something is wrong - keeps dt from blowing up, goes to zero to keep gyros from departing

// *** DCM algorithm

Read_adc_raw();
Matrix_update();
Normalize();
Drift_correction();
Euler_angles();

#if PRINT_BINARY == 1 && TWI_MODE == 0
printdata(); //Send info via serial
#endif

#if TWI_MODE
sendTWI();
#endif

//Turn on the LED when you saturate any of the gyros.
if((abs(Gyro_Vector[0])>=ToRad(300))||(abs(Gyro_Vector[1])>=ToRad(300))||(abs(Gyro_Vector[2])>=ToRad(300)))
{
gyro_sat=1;

#if PERFORMANCE_REPORTING == 1
gyro_sat_count++;
#endif
if(!SATGYRO_light)
{
digitalWrite(5,HIGH);
SATGYRO_light = 1;
}
}

cycleCount++;

// Do these things every 6th time through the main cycle
// This section gets called every 1000/(20*6) = 8 1/3 Hz
// doing it this way removes the need for another 'millis()' call
// and balances the processing load across main loop cycles.
switch (cycleCount) {
case(0):
GPS.update();
break;

case(1):
//Here we will check if we are getting a signal to ground start
if(digitalRead(GROUNDSTART_PIN) == LOW && groundstartDone == false)
startup_ground();
break;

case(2):
#if USE_BAROMETER==1
ReadSCP1000(); // Read I2C absolute pressure sensor
press_filt = (press + 2l * press_filt) / 3l; //Light filtering
//temperature = (temperature * 9 + temp_unfilt) / 10; We will just use the ground temp for the altitude calculation
#endif
break;

case(3):
#if USE_MAGNETOMETER==1
APM_Compass.Read(); // Read magnetometer
APM_Compass.Calculate(roll,pitch); // Calculate heading
#endif
break;

case(4):
// Display Status on LEDs
// GYRO Saturation indication
if(gyro_sat>=1)
{
SATGYRO_light = 1;
digitalWrite(5,HIGH); //Turn Red LED when gyro is saturated.
if(gyro_sat>=8) // keep the LED on for 8/10ths of a second
gyro_sat=0;
else
gyro_sat++;
}
else
{
if(SATGYRO_light)
{
digitalWrite(5,LOW);
SATGYRO_light = 0;
}
}

// YAW drift correction indication
if(GPS.ground_speed
{
if(!YAWDRIFT_light)
{
digitalWrite(7,HIGH); // Turn on yellow LED if speed too slow and yaw correction supressed
YAWDRIFT_light = 1;
}
}
else
{
if(YAWDRIFT_light)
{
digitalWrite(7,LOW);
YAWDRIFT_light = 0;
}
}

// GPS Fix indication
if(GPS.fix>=1)
{
if((!GPS_light))
{
digitalWrite(6,HIGH); //Turn Blue LED when gps is fixed.
GPS_light = 1;
}
}
else if (GPS.new_data == 1)
{
toggleMode = abs(toggleMode-1); // Toggle blue light on and off to indicate NMEA sentences exist, but no GPS fix lock
if (toggleMode==0)
{
digitalWrite(6, LOW); // Blue light off
GPS_light = 0;
}
else
{
digitalWrite(6, HIGH); // Blue light on
GPS_light = 1;
}
}
else
{
if((GPS_light))
{
digitalWrite(6,LOW);
GPS_light = 0;
}
}
break;

case(5):
cycleCount = -1; // Reset case counter, will be incremented to zero before switch statement
#if !PRINT_BINARY && !TWI_MODE
printdata(); //Send info via serial
#endif
break;
}


#if PERFORMANCE_REPORTING == 1
if (timeNow-perf_mon_timer > 20000)
{
printPerfData(timeNow-perf_mon_timer);
perf_mon_timer=timeNow;
}
#endif

}

}

//********************************************************************************
void startup_ground(void)
{
uint16_t store=0;
int flashcount = 0;

debug_handler(2);
for(int c=0; c
{
digitalWrite(7,LOW);
digitalWrite(6,HIGH);
digitalWrite(5,LOW);
delay(50);
Read_adc_raw();
digitalWrite(7,HIGH);
digitalWrite(6,LOW);
digitalWrite(5,HIGH);
delay(50);
}

Read_adc_raw();
delay(20);
Read_adc_raw();
for(int y=0; y<=5; y++) // Read first initial ADC values for offset.
AN_OFFSET[y]=AN[y];
#if USE_BAROMETER==1
ReadSCP1000();
press_gnd = press;
temperature = temp_unfilt;
delay(20);
#endif

for(int i=0;i<400;i++) // We take some readings...
{
Read_adc_raw();
for(int y=0; y<=5; y++) // Read initial ADC values for offset (averaging).
AN_OFFSET[y]=AN_OFFSET[y]*0.8 + AN[y]*0.2;
delay(20);
if(flashcount == 5) {
digitalWrite(7,LOW);
digitalWrite(6,HIGH);
digitalWrite(5,LOW);
}
if(flashcount >= 10) {
flashcount = 0;
#if USE_BAROMETER==1
ReadSCP1000();
press_gnd = (press_gnd * 9l + press) / 10l;
temperature = (temperature * 9 + temp_unfilt) / 10;

#endif
digitalWrite(7,HIGH);
digitalWrite(6,LOW);
digitalWrite(5,HIGH);
}
flashcount++;

}
digitalWrite(5,LOW);
digitalWrite(6,LOW);
digitalWrite(7,LOW);

AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];

for(int y=0; y<=5; y++)
{
Serial.println(AN_OFFSET[y]);
store = ((AN_OFFSET[y]-200.f)*100.0f);
eeprom_busy_wait();
eeprom_write_word((uint16_t *) (y*2+2), store);
}

while (gps_fix_count > 0 && USE_BAROMETER) {
GPS.update();
// Serial.print(gpsFix);
// Serial.print(", ");
// Serial.println(gpsFixnew);
if (GPS.fix == 1 && GPS.new_data == 1) {
GPS.new_data = 0;
gps_fix_count--;
}
}
#if USE_BAROMETER==1
press_filt = press_gnd;
ground_alt = GPS.altitude;
eeprom_busy_wait();
eeprom_write_dword((uint32_t *)0x10, press_gnd);
eeprom_busy_wait();
eeprom_write_word((uint16_t *)0x14, temperature);
eeprom_busy_wait();
eeprom_write_word((uint16_t *)0x16, (ground_alt/100));
#endif
groundstartDone = true;
debug_handler(6);
}

//************************************************************************************
void startup_air(void)
{
uint16_t temp=0;

for(int y=0; y<=5; y++)
{
eeprom_busy_wait();
temp = eeprom_read_word((uint16_t *) (y*2+2));
AN_OFFSET[y] = temp/100.f+200.f;
Serial.println(AN_OFFSET[y]);
}
#if USE_BAROMETER==1
eeprom_busy_wait();
press_gnd = eeprom_read_dword((uint32_t *) 0x10);
press_filt = press_gnd;
eeprom_busy_wait();
temperature = eeprom_read_word((uint16_t *) 0x14);
eeprom_busy_wait();
ground_alt = eeprom_read_word((uint16_t *) 0x16);
ground_alt *= 100;
#endif
Serial.println("***Air Start complete");
}


void debug_print(char string[])
{
#if PRINT_DEBUG != 0
Serial.print("???");
Serial.print(string);
Serial.println("***");
#endif
}

void debug_handler(byte message)
{
#if PRINT_DEBUG != 0

static unsigned long BAD_Checksum=0;

switch(message)
{
case 0:
Serial.print("???Software Version ");
Serial.print(SOFTWARE_VER);
Serial.println("***");
break;

case 1:
Serial.println("???Air Start!***");
break;

case 2:
Serial.println("???Ground Start!***");
break;

case 3:
Serial.println("???Enabling Magneto...***");
break;

case 4:
Serial.println("???Enabling Pressure Altitude...***");
break;

case 5:
Serial.println("???Air Start complete");
break;

case 6:
Serial.println("???Ground Start complete");
break;

case 10:
BAD_Checksum++;
Serial.print("???GPS Bad Checksum: ");
Serial.print(BAD_Checksum);
Serial.println("...***");
break;

default:
Serial.println("???Invalid debug ID...***");
break;

}
#endif

}

/*
EEPROM memory map

0 0x00 Unused
1 0x01 ..
2 0x02 AN_OFFSET[0]
3 0x03 ..
4 0x04 AN_OFFSET[1]
5 0x05 ..
6 0x06 AN_OFFSET[2]
7 0x07 ..
8 0x08 AN_OFFSET[3]
9 0x09 ..
10 0x0A AN_OFFSET[4]
11 0x0B ..
12 0x0C AN_OFFSET[5]
13 0x0D ..
14 0x0E Unused
15 0x0F ..
16 0x10 Ground Pressure
17 0x11 ..
18 0x12 ..
19 0x13 ..
20 0x14 Ground Temp
21 0x15 ..
22 0x16 Ground Altitude
23 0x17 ..
*/

*************END OF MAIN FUNCTION***************

*****TWI_COMM FUNCTION*****
/*
* twi_comm.pde
*
* Created on: Aug 2, 2010
* Author: markw
*/

//Modified:
//-----------------------
//! 2010-11-13 K.Bloch !
//-----------------------
//! Updated structures !
//! Reduced Protocol !
//! Overheads !
//! Integrated into IMU !
//-----------------------



#if TWI_MODE == 1

APdata apData;

//IPC Checksum
byte ck_a = 0;
byte ck_b = 0;
byte IMU_ck_a = 0;
byte IMU_ck_b = 0;

int payload_error_count = 0;
int checksum_error_count = 0;

void sendTWI()
{
// This section outputs binary data messages
// Conforms to new binary message standard (12/31/09)
byte IMU_buffer[23];
int tempint, t_roll, t_pitch;
int t_yaw;

int ck;
long templong;
byte IMU_ck_a=0;
byte IMU_ck_b=0;

t_roll=ToDeg(roll)*100; //Roll (degrees) * 100 in 2 bytes
t_pitch=ToDeg(pitch)*100; //Pitch (degrees) * 100 in 2 bytes
tempint=ToDeg(yaw)*100; //Yaw (degrees) * 100 in 2 bytes
if(tempint>18000) tempint -=36000;
if(tempint<-18000) tempint +=36000;
t_yaw = tempint;
//------------------------------------------GPS DATA----------------------------------------------------------------
// This section outputs the gps binary message when new gps data is available
if((GPS.new_data==1)&&(GPS.fix!=0)) {
#if PERFORMANCE_REPORTING == 1
gps_messages_sent++;
#endif

gps_sync_count = 0;
GPS.new_data=0;

IMU_buffer[0] = 0x04;
ck=21;
templong = GPS.longitude; //Longitude *10**7 in 4 bytes
IMU_buffer[1]=templong&0xff;
IMU_buffer[2]=(templong>>8)&0xff;
IMU_buffer[3]=(templong>>16)&0xff;
IMU_buffer[4]=(templong>>24)&0xff;

templong = GPS.latitude; //Latitude *10**7 in 4 bytes
IMU_buffer[5]=templong&0xff;
IMU_buffer[6]=(templong>>8)&0xff;
IMU_buffer[7]=(templong>>16)&0xff;
IMU_buffer[8]=(templong>>24)&0xff;

#if USE_BAROMETER==0
tempint=GPS.altitude / 100; // Altitude MSL in meters * 10 in 2 bytes
#else
alti();
tempint = (press_alt * ALT_MIX + GPS.altitude * (100-ALT_MIX)) / 10000; //Blended GPS and pressure altitude
#endif
IMU_buffer[9]=tempint&0xff;
IMU_buffer[10]=(tempint>>8)&0xff;

tempint=GPS.ground_speed; // Speed in M/S * 100 in 2 bytes
IMU_buffer[11]=tempint&0xff;
IMU_buffer[12]=(tempint>>8)&0xff;

tempint=GPS.ground_course; // course in degreees * 100 in 2 bytes
IMU_buffer[13]=tempint&0xff;
IMU_buffer[14]=(tempint>>8)&0xff;

IMU_buffer[15]=(imu_health>>8)&0xff;

IMU_buffer[16]=t_roll&0xff;
IMU_buffer[17]=(t_roll>>8)&0xff;
IMU_buffer[18]=t_pitch&0xff;
IMU_buffer[19]=(t_pitch>>8)&0xff;

IMU_buffer[20]=t_yaw&0xff;
IMU_buffer[21]=(t_yaw>>8)&0xff;

for (int i=1;i
IMU_ck_a+=IMU_buffer[i]; //Calculates checksums
IMU_ck_b+=IMU_ck_a;
}

IMU_buffer[22]=IMU_ck_a;
IMU_buffer[23]=IMU_ck_b;

Wire.beginTransmission(ARDUPILOT_ADDR); // transmit to device
for (int i=0;i
Wire.endTransmission(); // stop transmitting

} else {
//------------------------------------------IMU DATA----------------------------------------------------------------
// Serial.println("IMU"); // This is the message preamble
// This section outputs the IMU orientatiom message
IMU_buffer[0]=0x03;
ck=6;

IMU_buffer[1]=t_roll&0xff;
IMU_buffer[2]=(t_roll>>8)&0xff;
IMU_buffer[3]=t_pitch&0xff;
IMU_buffer[4]=(t_pitch>>8)&0xff;
IMU_buffer[5]=t_yaw&0xff;
IMU_buffer[6]=(t_yaw>>8)&0xff;

for (int i=1;i
IMU_ck_a+=IMU_buffer[i]; //Calculates checksums
IMU_ck_b+=IMU_ck_a;
}

IMU_buffer[7]=IMU_ck_a;
IMU_buffer[8]=IMU_ck_b;

Wire.beginTransmission(ARDUPILOT_ADDR); // transmit to device
for (int i=0;i
Wire.endTransmission(); // stop transmitting
}
}

#endif
********* END OF TWI_COMM FUNCTION *************

Comment by Greg Fletcher on November 22, 2010 at 7:09pm

Kevin, I don't think that you can include a file, only pics & links. It must be part of the blog entry and that wouldn't be good to scroll thru. You can only include a link to it(button left of camera button), That,s why you need to start a googlecode page. It's free and if I figured out how do do it I'm sure you can. Its password protected an only you can change it. Then include a link to it, which I still have trouble with if I haven't done it for a while.

Comment by Kevin Bloch on November 22, 2010 at 7:11pm

The Ardupilot IMU routine, thanks to MarkW for the original design. Hopefully this will give most people a start into TWI.

#if GPS_PROTOCOL == 6
// Performance Monitoring variables
// Data collected and reported for ~1 minute intervals
//int mainLoop_count = 0; //Main loop cycles since last report

byte IMU_buffer[30];
byte payload_length = 0;
byte payload_counter = 0;

//IMU Checksum
byte ck_a = 0;
byte ck_b = 0;
byte IMU_ck_a = 0;
byte IMU_ck_b = 0;

/****************************************************************
* Here you have all the stuff for data reception from the IMU_GPS
****************************************************************/

void init_gps(void)
{
GPS_fix = BAD_GPS;
}

/*
IMU Message format
Byte(s) Value
0-3 Header "DIYd"
4 Payload length = 6
5 Message ID = 2
6,7 roll Integer (degrees*100)
8,9 pitch Integer (degrees*100)
10,11 yaw Integer (degrees*100)
12,13 checksum

IMU Message format - new with Airspeed!
Byte(s) Value
0-3 Header "DIYd"
4 Payload length = 8
5 Message ID = 4
6,7 roll Integer (degrees*100)
8,9 pitch Integer (degrees*100)
10,11 yaw Integer (degrees*100)
12,13 airspeed Integer (meters*100)
14,15 checksum


GPS Message format
Byte(s) Value
0-3 Header "DIYd"
4 Payload length = 14
5 Message ID = 3
6-9 longitude Integer (value*10**7)
10-13 latitude Integer (value*10**7)
14,15 altitude Integer (meters*10)
16,17 gps speed Integer (M/S*100)
18,19 gps course not used
20,21 checksum
*/


IMUdata imuData;
GPSdata gpsData;

void decode_gps(void)
{
static unsigned long IMU_timer = 0; //used to set PROBLEM flag if no data is received.
if (newmsg){
newmsg = false;

if((DIYmillis() - IMU_timer) > 500)
{ //If we don't receive IMU data in 1/2 second, set flag
imu_ok = false;
}
}
}


void receiveEvent(int size)
{
// first byte is message ID
byte msgID = Wire.receive();
payload_counter=0;
ck_a = 0;
ck_b = 0;
uint32_t now = millis();
switch (msgID) {
case 0x03: //IMUdatafs

if((size == sizeof(imuData))&&(copyData(msgID, (uint8_t*)&imuData, size)))
{
roll_sensor = imuData.roll_sensor;
pitch_sensor = imuData.pitch_sensor;
ground_course = imuData.yaw_sensor;
imu_ok = true;
IMU_timer = DIYmillis(); //Restarting timer...
}
break;
case 0x04:
if((size == sizeof(gpsData))&&(copyData(msgID, (uint8_t*)&gpsData, size)))
{

GPS_timer = now;
gps_messages_received++;
current_loc.lng = gpsData.lng; // Lat and Lon * 10**7
current_loc.lat = gpsData.lat;
//Storing GPS Height above the sea level
current_loc.alt = (long)gpsData.alt * 10; // Altitude in meters * 100
//Storing Speed (3-D)
ground_speed = (float)gpsData.ground_speed; // Speed in cm/sec
// removed to shorten GPSdata struct below 32 bytes (TWI buffer length)
// iTOW = gpsData.gps_time;// Time of Week in milliseconds
// copy UTC (replaces TOW for NMEA GPS)
//memcpy(&gpsTime, &(gpsData.utc), sizeof(UTC));
imu_health = gpsData.imu_health;
roll_sensor = gpsData.roll_sensor;
pitch_sensor = gpsData.pitch_sensor;
ground_course = gpsData.yaw_sensor;
GPS_fix = VALID_GPS;
GPS_new_data = true;
imu_ok = true;
IMU_timer = DIYmillis(); //Restarting timer...
GPS_timer = DIYmillis(); //Restarting timer...
}
else
// Serial.println("GPS Data Failed");
break;
}

if((DIYmillis() - GPS_timer) > 2000)
{
if(GPS_fix != FAILED_GPS)
{
GPS_fix = BAD_GPS;
}
}
if((DIYmillis() - GPS_timer) > 10000)
{
GPS_fix = FAILED_GPS;
GPS_timer = DIYmillis();
// Serial.println("MSG \t No GPS, last 10s \t ***");
}
}


bool copyData(uint8_t msgID, uint8_t *IMU_buffer, int nbytes) {
bool valid = false;
int csum = 0;

ck_a=0;
ck_b=0;
//checksum(msgID);
IMU_buffer[0] = msgID;
for (int j=1; j
byte data = Wire.receive();
IMU_buffer[j] = data;
checksum(data);
}
IMU_ck_a = Wire.receive();
IMU_ck_b = Wire.receive();

if((ck_a == IMU_ck_a)&&(ck_b == IMU_ck_b))
valid = true;
else
{
newmsg = true;
valid = false;
}
return valid;
}

void checksum(byte data)
{
ck_a += data;
ck_b += ck_a;
}

// Join 4 bytes into a long
// -------------------------
int32_t join_4_bytes(byte Buffer[])
{
longUnion.byte[0] = *Buffer;
longUnion.byte[1] = *(Buffer+1);
longUnion.byte[2] = *(Buffer+2);
longUnion.byte[3] = *(Buffer+3);
return(longUnion.dword);
}

#endif

Comment by Kevin Bloch on November 22, 2010 at 7:14pm

Just be aware that this code is initial and untested in the air! Also, a bit of data has been stripped out to speed sending and processing and I don't think the TWI comms lost routines work yet. I will be improving on this unfinished code in the next few weeks and posting it as it progresses.

Comment by Kevin Bloch on November 22, 2010 at 7:18pm

ok Greg, noted a few minutes too late...
I'll have a look into starting a google code page but it would be nice to have an open area for people to book out code and upload their own changes to help progress this faster.

Comment by Greg Fletcher on November 22, 2010 at 7:35pm

Kevin, That exactly what the googlecode is for. Locationaly challenged teams to work on the same project. I have been working solo, but I'm sure you can give team members permissions. The'rs an SVN system that keeps track of changes, but I haven't used it yet. Like I said I am a total beginner as a coder, but I do think I have good ideas, I just struggle with the implementation. I have become very good at de-bugging due to all my typos and other errors :)

Comment by Greg Fletcher on November 22, 2010 at 10:31pm

Of course You will never see this due to the long scroll time and carpeltunel I warned you about before.
You Auses. Can you get me a job down there ?

Comment by Kevin Bloch on November 22, 2010 at 11:46pm

Yup, looks like I was drinking a little too much and threw up in ASCII !

Comment

• ‹ Previous
• 1
• 2
• Next ›
• Page