PDA

View Full Version : Teensyduino 1.20 Release Candidate #1 Available



Paul
07-03-2014, 12:18 PM
Here is a first release candidate for Teensyduino 1.20:


Old beta download links removed. Please use the latest version:
https://www.pjrc.com/teensy/td_download.html



Please give this a try and report any bugs. Try to include a sample program that reproduces the problem!

These are the changes since Teensyduino 1.19:


Add serialEvent (usb serial) & serialEvent# (hardware serial)
Add Serial.writeBufferFree() & Serial#.writeBufferFree()
Add IntervalTimer priority()
Add DMAChannel & DMASetting objects
Don't mix CPU speed setting switching from Teensy 2.0 to 3.x
Add FASTRUN, for functions in RAM on Teensy 3.x
Interrupt vector table moved to RAM for Teensy 3.x
Add attachInterruptVector()
Optimize Teensy 3.x hardware register definitions
Serial port names added to pins_arduino.h
Add AVR emulation for util/parity.h
Fix ARM toolchain linker & zlib issue (thanks to Andrew Kroll)
Add libraries: Adafruit_ILI9341 (optimized)
Updated libraries: Adafruit_SSD1306, Adafruit_ST7735, ShiftPWM
Libraries ported to Teensy 3.x: AltSoftSerial, CapacitiveSensor, FlexiTimer2, FreqCount, FreqMeasure, FrequencyTimer2, MsTimer2, ShiftPWM, TimerOne, TimerThree, Tlc5940

PaulStoffregen
07-03-2014, 09:52 PM
test reply

Nantonos
07-03-2014, 09:53 PM
Tested with fresh install of Arduino 1.0.5-r2 on Windows 7. No problems installing. Not noticed any breakage reversion so far.

A quick look at code sizes:

All tests with Arduino 1.0.5-r2

ssd1306_128x64_spi, Teensy 3.0, hardware spi

1.20-rc1
Binary sketch size: 24,848 bytes (of a 131,072 byte maximum)
Estimated memory use: 3,684 bytes (of a 16,384 byte maximum)

1.19
Binary sketch size: 27,784 bytes (of a 131,072 byte maximum)
Estimated memory use: 4,836 bytes (of a 16,384 byte maximum)

orgone_accumulator_official, Teensy 3.1

1.20-rc1
Binary sketch size: 63,204 bytes (of a 262,144 byte maximum)
Estimated memory use: 6,900 bytes (of a 65,536 byte maximum)

1.19
Binary sketch size: 63,436 bytes (of a 262,144 byte maximum)
Estimated memory use: 6,428 bytes (of a 65,536 byte maximum)

Nantonos
07-03-2014, 09:53 PM
(restarted browser, works now)

Nantonos
07-03-2014, 09:56 PM
I'm not seeing the Czech keyboard in the Arduino menu. According to this thread (http://forum.pjrc.com/threads/24495-Czech-keyboard?highlight=czech) it is tested as working.

PaulStoffregen
07-03-2014, 10:20 PM
Opps, yeah, Czech is in the code but was never added to boards.txt. I'll get it into rc2.

onehorse
07-04-2014, 02:45 AM
Just downloaded the new version and fired up a sketch I wrote today for AVR and it's working beautifully on the Teensy 3.1. I noticed that now there are a greater variety of clock speeds available. Does 'no USB' mean that one cannot use the USB in a program at these low clock speeds or that one cannot enter a new program through the USB at these low clock speed settings? Could you comment on this increase in clock speed setting choices? Thanks.

onehorse
07-04-2014, 06:08 PM
I'm getting this error message when I try to compile using the i2c_t3.h library, which was working for me with the previous 1.18 version on teensyduino. I am going back to v 1.19 to try and see if it's the new version or my new laptop.


This report would have more information with
"Show verbose output during compilation"
enabled in File > Preferences.
Arduino: 1.0.5-r2 (Windows 7), Board: "Teensy 3.1"
MPU9250BasicAHRS_t3.cpp.o: In function `writeByte(unsigned char, unsigned char, unsigned char)':
C:\Program Files\Arduino/quaternionFilters.ino:192: undefined reference to `i2c_t3::beginTransmission(unsigned char)'
C:\Program Files\Arduino/quaternionFilters.ino:192: undefined reference to `i2c_t3::write(unsigned char)'
C:\Program Files\Arduino/quaternionFilters.ino:192: undefined reference to `i2c_t3::write(unsigned char)'
MPU9250BasicAHRS_t3.cpp.o: In function `writeByte(unsigned char, unsigned char, unsigned char)':
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:965: undefined reference to `Wire'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:965: undefined reference to `i2c_t3::endTransmission()'
MPU9250BasicAHRS_t3.cpp.o: In function `readByte(unsigned char, unsigned char)':
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:971: undefined reference to `i2c_t3::beginTransmission(unsigned char)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:972: undefined reference to `i2c_t3::write(unsigned char)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:973: undefined reference to `i2c_t3::endTransmission(i2c_stop)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:976: undefined reference to `i2c_t3::requestFrom(unsigned char, unsigned int)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:977: undefined reference to `i2c_t3::read()'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:979: undefined reference to `Wire'
MPU9250BasicAHRS_t3.cpp.o: In function `readBytes(unsigned char, unsigned char, unsigned char, unsigned char*)':
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:983: undefined reference to `i2c_t3::beginTransmission(unsigned char)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:984: undefined reference to `i2c_t3::write(unsigned char)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:985: undefined reference to `i2c_t3::endTransmission(i2c_stop)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:989: undefined reference to `i2c_t3::requestFrom(unsigned char, unsigned int)'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:991: undefined reference to `i2c_t3::read()'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:990: undefined reference to `i2c_t3::available()'
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:992: undefined reference to `Wire'
MPU9250BasicAHRS_t3.cpp.o: In function `setup':
C:\Program Files\Arduino/MPU9250BasicAHRS_t3.ino:276: undefined reference to `i2c_t3::begin(i2c_mode, unsigned char, i2c_pins, i2c_pullup, i2c_rate)'
MPU9250BasicAHRS_t3.cpp.o: In function `Print::println(int, int)':
C:\Program Files\Arduino\hardware\teensy\cores\teensy3/Print.h:89: undefined reference to `Wire'
collect2.exe: error: ld returned 1 exit statusThis is the program I am running:


/* MPU9250 Basic Example Code
by: Kris Winer
date: April 1, 2014
license: Beerware - Use this code however you'd like. If you
find it useful you can buy me a beer some time.

Demonstrate basic MPU-9250 functionality including parameterizing the register addresses, initializing the sensor,
getting properly scaled accelerometer, gyroscope, and magnetometer data out. Added display functions to
allow display to on breadboard monitor. Addition of 9 DoF sensor fusion using open source Madgwick and
Mahony filter algorithms. Sketch runs on the 3.3 V 8 MHz Pro Mini and the Teensy 3.1.

SDA and SCL should have external pull-up resistors (to 3.3V).
10k resistors are on the EMSENSR-9250 breakout board.

Hardware setup:
MPU9250 Breakout --------- Arduino
VDD ---------------------- 3.3V
VDDI --------------------- 3.3V
SDA ----------------------- A4
SCL ----------------------- A5
GND ---------------------- GND

Note: The MPU9250 is an I2C sensor and uses the Arduino Wire library.
Because the sensor is not 5V tolerant, we are using a 3.3 V 8 MHz Pro Mini or a 3.3 V Teensy 3.1.
We have disabled the internal pull-ups used by the Wire library in the Wire.h/twi.c utility file.
We are also using the 400 kHz fast I2C mode by setting the TWI_FREQ to 400000L /twi.h utility file.
*/
//#include "Wire.h"
#include <i2c_t3.h>
#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_PCD8544.h>

// Using NOKIA 5110 monochrome 84 x 48 pixel display
// pin 9 - Serial clock out (SCLK)
// pin 8 - Serial data out (DIN)
// pin 7 - Data/Command select (D/C)
// pin 5 - LCD chip select (CS)
// pin 6 - LCD reset (RST)
Adafruit_PCD8544 display = Adafruit_PCD8544(9, 8, 7, 5, 6);

// See also MPU-9250 Register Map and Descriptions, Revision 4.0, RM-MPU-9250A-00, Rev. 1.4, 9/9/2013 for registers not listed in
// above document; the MPU9250 and MPU9150 are virtually identical but the latter has a different register map
//
//Magnetometer Registers
#define AK8963_ADDRESS 0x0C
#define WHO_AM_I_AK8963 0x00 // should return 0x48
#define INFO 0x01
#define AK8963_ST1 0x02 // data ready status bit 0
#define AK8963_XOUT_L 0x03 // data
#define AK8963_XOUT_H 0x04
#define AK8963_YOUT_L 0x05
#define AK8963_YOUT_H 0x06
#define AK8963_ZOUT_L 0x07
#define AK8963_ZOUT_H 0x08
#define AK8963_ST2 0x09 // Data overflow bit 3 and data read error status bit 2
#define AK8963_CNTL 0x0A // Power down (0000), single-measurement (0001), self-test (1000) and Fuse ROM (1111) modes on bits 3:0
#define AK8963_ASTC 0x0C // Self test control
#define AK8963_I2CDIS 0x0F // I2C disable
#define AK8963_ASAX 0x10 // Fuse ROM x-axis sensitivity adjustment value
#define AK8963_ASAY 0x11 // Fuse ROM y-axis sensitivity adjustment value
#define AK8963_ASAZ 0x12 // Fuse ROM z-axis sensitivity adjustment value

#define SELF_TEST_X_GYRO 0x00
#define SELF_TEST_Y_GYRO 0x01
#define SELF_TEST_Z_GYRO 0x02

/*#define X_FINE_GAIN 0x03 // [7:0] fine gain
#define Y_FINE_GAIN 0x04
#define Z_FINE_GAIN 0x05
#define XA_OFFSET_H 0x06 // User-defined trim values for accelerometer
#define XA_OFFSET_L_TC 0x07
#define YA_OFFSET_H 0x08
#define YA_OFFSET_L_TC 0x09
#define ZA_OFFSET_H 0x0A
#define ZA_OFFSET_L_TC 0x0B */

#define SELF_TEST_X_ACCEL 0x0D
#define SELF_TEST_Y_ACCEL 0x0E
#define SELF_TEST_Z_ACCEL 0x0F

#define SELF_TEST_A 0x10

#define XG_OFFSET_H 0x13 // User-defined trim values for gyroscope
#define XG_OFFSET_L 0x14
#define YG_OFFSET_H 0x15
#define YG_OFFSET_L 0x16
#define ZG_OFFSET_H 0x17
#define ZG_OFFSET_L 0x18
#define SMPLRT_DIV 0x19
#define CONFIG 0x1A
#define GYRO_CONFIG 0x1B
#define ACCEL_CONFIG 0x1C
#define ACCEL_CONFIG2 0x1D
#define LP_ACCEL_ODR 0x1E
#define WOM_THR 0x1F

#define MOT_DUR 0x20 // Duration counter threshold for motion interrupt generation, 1 kHz rate, LSB = 1 ms
#define ZMOT_THR 0x21 // Zero-motion detection threshold bits [7:0]
#define ZRMOT_DUR 0x22 // Duration counter threshold for zero motion interrupt generation, 16 Hz rate, LSB = 64 ms

#define FIFO_EN 0x23
#define I2C_MST_CTRL 0x24
#define I2C_SLV0_ADDR 0x25
#define I2C_SLV0_REG 0x26
#define I2C_SLV0_CTRL 0x27
#define I2C_SLV1_ADDR 0x28
#define I2C_SLV1_REG 0x29
#define I2C_SLV1_CTRL 0x2A
#define I2C_SLV2_ADDR 0x2B
#define I2C_SLV2_REG 0x2C
#define I2C_SLV2_CTRL 0x2D
#define I2C_SLV3_ADDR 0x2E
#define I2C_SLV3_REG 0x2F
#define I2C_SLV3_CTRL 0x30
#define I2C_SLV4_ADDR 0x31
#define I2C_SLV4_REG 0x32
#define I2C_SLV4_DO 0x33
#define I2C_SLV4_CTRL 0x34
#define I2C_SLV4_DI 0x35
#define I2C_MST_STATUS 0x36
#define INT_PIN_CFG 0x37
#define INT_ENABLE 0x38
#define DMP_INT_STATUS 0x39 // Check DMP interrupt
#define INT_STATUS 0x3A
#define ACCEL_XOUT_H 0x3B
#define ACCEL_XOUT_L 0x3C
#define ACCEL_YOUT_H 0x3D
#define ACCEL_YOUT_L 0x3E
#define ACCEL_ZOUT_H 0x3F
#define ACCEL_ZOUT_L 0x40
#define TEMP_OUT_H 0x41
#define TEMP_OUT_L 0x42
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44
#define GYRO_YOUT_H 0x45
#define GYRO_YOUT_L 0x46
#define GYRO_ZOUT_H 0x47
#define GYRO_ZOUT_L 0x48
#define EXT_SENS_DATA_00 0x49
#define EXT_SENS_DATA_01 0x4A
#define EXT_SENS_DATA_02 0x4B
#define EXT_SENS_DATA_03 0x4C
#define EXT_SENS_DATA_04 0x4D
#define EXT_SENS_DATA_05 0x4E
#define EXT_SENS_DATA_06 0x4F
#define EXT_SENS_DATA_07 0x50
#define EXT_SENS_DATA_08 0x51
#define EXT_SENS_DATA_09 0x52
#define EXT_SENS_DATA_10 0x53
#define EXT_SENS_DATA_11 0x54
#define EXT_SENS_DATA_12 0x55
#define EXT_SENS_DATA_13 0x56
#define EXT_SENS_DATA_14 0x57
#define EXT_SENS_DATA_15 0x58
#define EXT_SENS_DATA_16 0x59
#define EXT_SENS_DATA_17 0x5A
#define EXT_SENS_DATA_18 0x5B
#define EXT_SENS_DATA_19 0x5C
#define EXT_SENS_DATA_20 0x5D
#define EXT_SENS_DATA_21 0x5E
#define EXT_SENS_DATA_22 0x5F
#define EXT_SENS_DATA_23 0x60
#define MOT_DETECT_STATUS 0x61
#define I2C_SLV0_DO 0x63
#define I2C_SLV1_DO 0x64
#define I2C_SLV2_DO 0x65
#define I2C_SLV3_DO 0x66
#define I2C_MST_DELAY_CTRL 0x67
#define SIGNAL_PATH_RESET 0x68
#define MOT_DETECT_CTRL 0x69
#define USER_CTRL 0x6A // Bit 7 enable DMP, bit 3 reset DMP
#define PWR_MGMT_1 0x6B // Device defaults to the SLEEP mode
#define PWR_MGMT_2 0x6C
#define DMP_BANK 0x6D // Activates a specific bank in the DMP
#define DMP_RW_PNT 0x6E // Set read/write pointer to a specific start address in specified DMP bank
#define DMP_REG 0x6F // Register in DMP from which to read or to which to write
#define DMP_REG_1 0x70
#define DMP_REG_2 0x71
#define FIFO_COUNTH 0x72
#define FIFO_COUNTL 0x73
#define FIFO_R_W 0x74
#define WHO_AM_I_MPU9250 0x75 // Should return 0x71
#define XA_OFFSET_H 0x77
#define XA_OFFSET_L 0x78
#define YA_OFFSET_H 0x7A
#define YA_OFFSET_L 0x7B
#define ZA_OFFSET_H 0x7D
#define ZA_OFFSET_L 0x7E

// Using the MSENSR-9250 breakout board, ADO is set to 0
// Seven-bit device address is 110100 for ADO = 0 and 110101 for ADO = 1
#define ADO 0
#if ADO
#define MPU9250_ADDRESS 0x69 // Device address when ADO = 1
#else
#define MPU9250_ADDRESS 0x68 // Device address when ADO = 0
#define AK8963_ADDRESS 0x0C // Address of magnetometer
#endif

#define AHRS true // set to false for basic data read
#define SerialDebug true // set to true to get Serial output for debugging

// Set initial input parameters
enum Ascale {
AFS_2G = 0,
AFS_4G,
AFS_8G,
AFS_16G
};

enum Gscale {
GFS_250DPS = 0,
GFS_500DPS,
GFS_1000DPS,
GFS_2000DPS
};

enum Mscale {
MFS_14BITS = 0, // 0.6 mG per LSB
MFS_16BITS // 0.15 mG per LSB
};

// Specify sensor full scale
uint8_t Gscale = GFS_250DPS;
uint8_t Ascale = AFS_2G;
uint8_t Mscale = MFS_16BITS; // Choose either 14-bit or 16-bit magnetometer resolution
uint8_t Mmode = 0x02; // 2 for 8 Hz, 6 for 100 Hz continuous magnetometer data read
float aRes, gRes, mRes; // scale resolutions per LSB for the sensors

// Pin definitions
int intPin = 12; // These can be changed, 2 and 3 are the Arduinos ext int pins

int16_t accelCount[3]; // Stores the 16-bit signed accelerometer sensor output
int16_t gyroCount[3]; // Stores the 16-bit signed gyro sensor output
int16_t magCount[3]; // Stores the 16-bit signed magnetometer sensor output
float magCalibration[3] = {0, 0, 0}, magbias[3] = {0, 0, 0}; // Factory mag calibration and mag bias
float gyroBias[3] = {0, 0, 0}, accelBias[3] = {0, 0, 0}; // Bias corrections for gyro and accelerometer
int16_t tempCount; // temperature raw count output
float temperature; // Stores the real internal chip temperature in degrees Celsius
float SelfTest[6]; // holds results of gyro and accelerometer self test

// global constants for 9 DoF fusion and AHRS (Attitude and Heading Reference System)
float GyroMeasError = PI * (40.0f / 180.0f); // gyroscope measurement error in rads/s (start at 40 deg/s)
float GyroMeasDrift = PI * (0.0f / 180.0f); // gyroscope measurement drift in rad/s/s (start at 0.0 deg/s/s)
// There is a tradeoff in the beta parameter between accuracy and response speed.
// In the original Madgwick study, beta of 0.041 (corresponding to GyroMeasError of 2.7 degrees/s) was found to give optimal accuracy.
// However, with this value, the LSM9SD0 response time is about 10 seconds to a stable initial quaternion.
// Subsequent changes also require a longish lag time to a stable output, not fast enough for a quadcopter or robot car!
// By increasing beta (GyroMeasError) by about a factor of fifteen, the response time constant is reduced to ~2 sec
// I haven't noticed any reduction in solution accuracy. This is essentially the I coefficient in a PID control sense;
// the bigger the feedback coefficient, the faster the solution converges, usually at the expense of accuracy.
// In any case, this is the free parameter in the Madgwick filtering and fusion scheme.
float beta = sqrt(3.0f / 4.0f) * GyroMeasError; // compute beta
float zeta = sqrt(3.0f / 4.0f) * GyroMeasDrift; // compute zeta, the other free parameter in the Madgwick scheme usually set to a small or zero value
#define Kp 2.0f * 5.0f // these are the free parameters in the Mahony filter and fusion scheme, Kp for proportional feedback, Ki for integral
#define Ki 0.0f

uint32_t delt_t = 0; // used to control display output rate
uint32_t count = 0, sumCount = 0; // used to control display output rate
float pitch, yaw, roll;
float deltat = 0.0f, sum = 0.0f; // integration interval for both filter schemes
uint32_t lastUpdate = 0, firstUpdate = 0; // used to calculate integration interval
uint32_t Now = 0; // used to calculate integration interval

float ax, ay, az, gx, gy, gz, mx, my, mz; // variables to hold latest sensor data values
float q[4] = {1.0f, 0.0f, 0.0f, 0.0f}; // vector to hold quaternion
float eInt[3] = {0.0f, 0.0f, 0.0f}; // vector to hold integral error for Mahony method


void setup()
{
// Wire.begin();
// TWBR = 12; // 400 kbit/sec I2C speed
// Setup for Master mode, pins 18/19, external pullups, 400kHz
Wire.begin(I2C_MASTER, 0x00, I2C_PINS_18_19, I2C_PULLUP_EXT, I2C_RATE_400);
Serial.begin(38400);

// Set up the interrupt pin, its set as active high, push-pull
pinMode(intPin, INPUT);
digitalWrite(intPin, LOW);

display.begin(); // Ini8ialize the display
display.setContrast(58); // Set the contrast

// Start device display with ID of sensor
display.clearDisplay();
display.setTextSize(2);
display.setCursor(0,0); display.print("MPU9250");
display.setTextSize(1);
display.setCursor(0, 20); display.print("9-DOF 16-bit");
display.setCursor(0, 30); display.print("motion sensor");
display.setCursor(20,40); display.print("60 ug LSB");
display.display();
delay(1000);

// Set up for data display
display.setTextSize(1); // Set text size to normal, 2 is twice normal etc.
display.setTextColor(BLACK); // Set pixel color; 1 on the monochrome screen
display.clearDisplay(); // clears the screen and buffer

// Read the WHO_AM_I register, this is a good test of communication
byte c = readByte(MPU9250_ADDRESS, WHO_AM_I_MPU9250); // Read WHO_AM_I register for MPU-9250
Serial.print("MPU9250 "); Serial.print("I AM "); Serial.print(c, HEX); Serial.print(" I should be "); Serial.println(0x71, HEX);
display.setCursor(20,0); display.print("MPU9250");
display.setCursor(0,10); display.print("I AM");
display.setCursor(0,20); display.print(c, HEX);
display.setCursor(0,30); display.print("I Should Be");
display.setCursor(0,40); display.print(0x71, HEX);
display.display();
delay(1000);

if (c == 0x71) // WHO_AM_I should always be 0x68
{
Serial.println("MPU9250 is online...");

/* MPU6050SelfTest(SelfTest); // Start by performing self test and reporting values
Serial.print("x-axis self test: acceleration trim within : "); Serial.print(SelfTest[0],1); Serial.println("% of factory value");
Serial.print("y-axis self test: acceleration trim within : "); Serial.print(SelfTest[1],1); Serial.println("% of factory value");
Serial.print("z-axis self test: acceleration trim within : "); Serial.print(SelfTest[2],1); Serial.println("% of factory value");
Serial.print("x-axis self test: gyration trim within : "); Serial.print(SelfTest[3],1); Serial.println("% of factory value");
Serial.print("y-axis self test: gyration trim within : "); Serial.print(SelfTest[4],1); Serial.println("% of factory value");
Serial.print("z-axis self test: gyration trim within : "); Serial.print(SelfTest[5],1); Serial.println("% of factory value");

*/
calibrateMPU9250(gyroBias, accelBias); // Calibrate gyro and accelerometers, load biases in bias registers
display.clearDisplay();

display.setCursor(0, 0); display.print("MPU9250 bias");
display.setCursor(0, 8); display.print(" x y z ");

display.setCursor(0, 16); display.print((int)(1000*accelBias[0]));
display.setCursor(24, 16); display.print((int)(1000*accelBias[1]));
display.setCursor(48, 16); display.print((int)(1000*accelBias[2]));
display.setCursor(72, 16); display.print("mg");

display.setCursor(0, 24); display.print(gyroBias[0], 1);
display.setCursor(24, 24); display.print(gyroBias[1], 1);
display.setCursor(48, 24); display.print(gyroBias[2], 1);
display.setCursor(66, 24); display.print("o/s");

display.display();
delay(1000);

initMPU9250();
Serial.println("MPU9250 initialized for active data mode...."); // Initialize device for active mode read of acclerometer, gyroscope, and temperature

// Read the WHO_AM_I register of the magnetometer, this is a good test of communication
byte d = readByte(AK8963_ADDRESS, WHO_AM_I_AK8963); // Read WHO_AM_I register for AK8963
Serial.print("AK8963 "); Serial.print("I AM "); Serial.print(d, HEX); Serial.print(" I should be "); Serial.println(0x48, HEX);
display.clearDisplay();
display.setCursor(20,0); display.print("AK8963");
display.setCursor(0,10); display.print("I AM");
display.setCursor(0,20); display.print(d, HEX);
display.setCursor(0,30); display.print("I Should Be");
display.setCursor(0,40); display.print(0x48, HEX);
display.display();
delay(1000);

// Get magnetometer calibration from AK8963 ROM
initAK8963(magCalibration); Serial.println("AK8963 initialized for active data mode...."); // Initialize device for active mode read of magnetometer

if(SerialDebug) {
//// Serial.println("Calibration values: ");
// Serial.print("X-Axis sensitivity adjustment value "); Serial.println(magCalibration[0], 2);
// Serial.print("Y-Axis sensitivity adjustment value "); Serial.println(magCalibration[1], 2);
// Serial.print("Z-Axis sensitivity adjustment value "); Serial.println(magCalibration[2], 2);
}

display.clearDisplay();
display.setCursor(20,0); display.print("AK8963");
display.setCursor(0,10); display.print("ASAX "); display.setCursor(50,10); display.print(magCalibration[0], 2);
display.setCursor(0,20); display.print("ASAY "); display.setCursor(50,20); display.print(magCalibration[1], 2);
display.setCursor(0,30); display.print("ASAZ "); display.setCursor(50,30); display.print(magCalibration[2], 2);
display.display();
delay(1000);
}
else
{
Serial.print("Could not connect to MPU9250: 0x");
Serial.println(c, HEX);
while(1) ; // Loop forever if communication doesn't happen
}
}

void loop()
{
// If intPin goes high, all data registers have new data
if (readByte(MPU9250_ADDRESS, INT_STATUS) & 0x01) { // On interrupt, check if data ready interrupt
readAccelData(accelCount); // Read the x/y/z adc values
getAres();

// Now we'll calculate the accleration value into actual g's
ax = (float)accelCount[0]*aRes - accelBias[0]; // get actual g value, this depends on scale being set
ay = (float)accelCount[1]*aRes - accelBias[1];
az = (float)accelCount[2]*aRes - accelBias[2];

readGyroData(gyroCount); // Read the x/y/z adc values
getGres();

// Calculate the gyro value into actual degrees per second
gx = (float)gyroCount[0]*gRes; // get actual gyro value, this depends on scale being set
gy = (float)gyroCount[1]*gRes;
gz = (float)gyroCount[2]*gRes;

readMagData(magCount); // Read the x/y/z adc values
getMres();
magbias[0] = +470.; // User environmental x-axis correction in milliGauss, should be automatically calculated
magbias[1] = +120.; // User environmental x-axis correction in milliGauss
magbias[2] = +125.; // User environmental x-axis correction in milliGauss

// Calculate the magnetometer values in milliGauss
// Include factory calibration per data sheet and user environmental corrections
mx = (float)magCount[0]*mRes*magCalibration[0] - magbias[0]; // get actual magnetometer value, this depends on scale being set
my = (float)magCount[1]*mRes*magCalibration[1] - magbias[1];
mz = (float)magCount[2]*mRes*magCalibration[2] - magbias[2];
}

Now = micros();
deltat = ((Now - lastUpdate)/1000000.0f); // set integration time by time elapsed since last filter update
lastUpdate = Now;

sum += deltat; // sum for averaging filter update rate
sumCount++;

// Sensors x (y)-axis of the accelerometer is aligned with the y (x)-axis of the magnetometer;
// the magnetometer z-axis (+ down) is opposite to z-axis (+ up) of accelerometer and gyro!
// We have to make some allowance for this orientationmismatch in feeding the output to the quaternion filter.
// For the MPU-9250, we have chosen a magnetic rotation that keeps the sensor forward along the x-axis just like
// in the LSM9DS0 sensor. This rotation can be modified to allow any convenient orientation convention.
// This is ok by aircraft orientation standards!
// Pass gyro rate as rad/s
MadgwickQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);
// MahonyQuaternionUpdate(ax, ay, az, gx*PI/180.0f, gy*PI/180.0f, gz*PI/180.0f, my, mx, mz);


if (!AHRS) {
delt_t = millis() - count;
if(delt_t > 500) {

if(SerialDebug) {
// Print acceleration values in milligs!
Serial.print("X-acceleration: "); Serial.print(1000*ax); Serial.print(" mg ");
Serial.print("Y-acceleration: "); Serial.print(1000*ay); Serial.print(" mg ");
Serial.print("Z-acceleration: "); Serial.print(1000*az); Serial.println(" mg ");

// Print gyro values in degree/sec
Serial.print("X-gyro rate: "); Serial.print(gx, 3); Serial.print(" degrees/sec ");
Serial.print("Y-gyro rate: "); Serial.print(gy, 3); Serial.print(" degrees/sec ");
Serial.print("Z-gyro rate: "); Serial.print(gz, 3); Serial.println(" degrees/sec");

// Print mag values in degree/sec
Serial.print("X-mag field: "); Serial.print(mx); Serial.print(" mG ");
Serial.print("Y-mag field: "); Serial.print(my); Serial.print(" mG ");
Serial.print("Z-mag field: "); Serial.print(mz); Serial.println(" mG");

tempCount = readTempData(); // Read the adc values
temperature = ((float) tempCount) / 333.87 + 21.0; // Temperature in degrees Centigrade
// Print temperature in degrees Centigrade
Serial.print("Temperature is "); Serial.print(temperature, 1); Serial.println(" degrees C"); // Print T values to tenths of s degree C
}

display.clearDisplay();
display.setCursor(0, 0); display.print("MPU9250/AK8963");
display.setCursor(0, 8); display.print(" x y z ");

display.setCursor(0, 16); display.print((int)(1000*ax));
display.setCursor(24, 16); display.print((int)(1000*ay));
display.setCursor(48, 16); display.print((int)(1000*az));
display.setCursor(72, 16); display.print("mg");

display.setCursor(0, 24); display.print((int)(gx));
display.setCursor(24, 24); display.print((int)(gy));
display.setCursor(48, 24); display.print((int)(gz));
display.setCursor(66, 24); display.print("o/s");

display.setCursor(0, 32); display.print((int)(mx));
display.setCursor(24, 32); display.print((int)(my));
display.setCursor(48, 32); display.print((int)(mz));
display.setCursor(72, 32); display.print("mG");

display.setCursor(0, 40); display.print("Gyro T ");
display.setCursor(50, 40); display.print(temperature, 1); display.print(" C");
display.display();

count = millis();
}
}
else {

// Serial print and/or display at 0.5 s rate independent of data rates
delt_t = millis() - count;
if (delt_t > 500) { // update LCD once per half-second independent of read rate
/*
if(SerialDebug) {
Serial.print("ax = "); Serial.print((int)1000*ax);
Serial.print(" ay = "); Serial.print((int)1000*ay);
Serial.print(" az = "); Serial.print((int)1000*az); Serial.println(" mg");
Serial.print("gx = "); Serial.print( gx, 2);
Serial.print(" gy = "); Serial.print( gy, 2);
Serial.print(" gz = "); Serial.print( gz, 2); Serial.println(" deg/s");
Serial.print("mx = "); Serial.print( (int)mx );
Serial.print(" my = "); Serial.print( (int)my );
Serial.print(" mz = "); Serial.print( (int)mz ); Serial.println(" mG");

Serial.print("q0 = "); Serial.print(q[0]);
Serial.print(" qx = "); Serial.print(q[1]);
Serial.print(" qy = "); Serial.print(q[2]);
Serial.print(" qz = "); Serial.println(q[3]);
}
*/
// Define output variables from updated quaternion---these are Tait-Bryan angles, commonly used in aircraft orientation.
// In this coordinate system, the positive z-axis is down toward Earth.
// Yaw is the angle between Sensor x-axis and Earth magnetic North (or true North if corrected for local declination, looking down on the sensor positive yaw is counterclockwise.
// Pitch is angle between sensor x-axis and Earth ground plane, toward the Earth is positive, up toward the sky is negative.
// Roll is angle between sensor y-axis and Earth ground plane, y-axis up is positive roll.
// These arise from the definition of the homogeneous rotation matrix constructed from quaternions.
// Tait-Bryan angles as well as Euler angles are non-commutative; that is, the get the correct orientation the rotations must be
// applied in the correct order which for this configuration is yaw, pitch, and then roll.
// For more see http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles which has additional links.
yaw = atan2(2.0f * (q[1] * q[2] + q[0] * q[3]), q[0] * q[0] + q[1] * q[1] - q[2] * q[2] - q[3] * q[3]);
pitch = -asin(2.0f * (q[1] * q[3] - q[0] * q[2]));
roll = atan2(2.0f * (q[0] * q[1] + q[2] * q[3]), q[0] * q[0] - q[1] * q[1] - q[2] * q[2] + q[3] * q[3]);
pitch *= 180.0f / PI;
yaw *= 180.0f / PI;
yaw -= 13.8; // Declination at Danville, California is 13 degrees 48 minutes and 47 seconds on 2014-04-04
roll *= 180.0f / PI;

if(SerialDebug) {
// Serial.print("Yaw, Pitch, Roll: ");
Serial.print(yaw, 2);
Serial.print(", ");
Serial.print(pitch, 2);
Serial.print(", ");
Serial.println(roll, 2);

// Serial.print("rate = "); Serial.print((float)sumCount/sum, 2); Serial.println(" Hz");
}

display.clearDisplay();

display.setCursor(0, 0); display.print(" x y z ");

display.setCursor(0, 8); display.print((int)(1000*ax));
display.setCursor(24, 8); display.print((int)(1000*ay));
display.setCursor(48, 8); display.print((int)(1000*az));
display.setCursor(72, 8); display.print("mg");

display.setCursor(0, 16); display.print((int)(gx));
display.setCursor(24, 16); display.print((int)(gy));
display.setCursor(48, 16); display.print((int)(gz));
display.setCursor(66, 16); display.print("o/s");

display.setCursor(0, 24); display.print((int)(mx));
display.setCursor(24, 24); display.print((int)(my));
display.setCursor(48, 24); display.print((int)(mz));
display.setCursor(72, 24); display.print("mG");

display.setCursor(0, 32); display.print((int)(yaw));
display.setCursor(24, 32); display.print((int)(pitch));
display.setCursor(48, 32); display.print((int)(roll));
display.setCursor(66, 32); display.print("ypr");

// With these settings the filter is updating at a ~145 Hz rate using the Madgwick scheme and
// >200 Hz using the Mahony scheme even though the display refreshes at only 2 Hz.
// The filter update rate is determined mostly by the mathematical steps in the respective algorithms,
// the processor speed (8 MHz for the 3.3V Pro Mini), and the magnetometer ODR:
// an ODR of 10 Hz for the magnetometer produce the above rates, maximum magnetometer ODR of 100 Hz produces
// filter update rates of 36 - 145 and ~38 Hz for the Madgwick and Mahony schemes, respectively.
// This is presumably because the magnetometer read takes longer than the gyro or accelerometer reads.
// This filter update rate should be fast enough to maintain accurate platform orientation for
// stabilization control of a fast-moving robot or quadcopter. Compare to the update rate of 200 Hz
// produced by the on-board Digital Motion Processor of Invensense's MPU6050 6 DoF and MPU9150 9DoF sensors.
// The 3.3 V 8 MHz Pro Mini is doing pretty well!
display.setCursor(0, 40); display.print("rt: "); display.print((float) sumCount / sum, 2); display.print(" Hz");
display.display();

count = millis();
sumCount = 0;
sum = 0;
}
}

}

//================================================== ================================================== ===============
//====== Set of useful function to access acceleration. gyroscope, magnetometer, and temperature data
//================================================== ================================================== ===============

void getMres() {
switch (Mscale)
{
// Possible magnetometer scales (and their register bit settings) are:
// 14 bit resolution (0) and 16 bit resolution (1)
case MFS_14BITS:
mRes = 10.*4219./8190.; // Proper scale to return milliGauss
break;
case MFS_16BITS:
mRes = 10.*4219./32760.0; // Proper scale to return milliGauss
break;
}
}

void getGres() {
switch (Gscale)
{
// Possible gyro scales (and their register bit settings) are:
// 250 DPS (00), 500 DPS (01), 1000 DPS (10), and 2000 DPS (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case GFS_250DPS:
gRes = 250.0/32768.0;
break;
case GFS_500DPS:
gRes = 500.0/32768.0;
break;
case GFS_1000DPS:
gRes = 1000.0/32768.0;
break;
case GFS_2000DPS:
gRes = 2000.0/32768.0;
break;
}
}

void getAres() {
switch (Ascale)
{
// Possible accelerometer scales (and their register bit settings) are:
// 2 Gs (00), 4 Gs (01), 8 Gs (10), and 16 Gs (11).
// Here's a bit of an algorith to calculate DPS/(ADC tick) based on that 2-bit value:
case AFS_2G:
aRes = 2.0/32768.0;
break;
case AFS_4G:
aRes = 4.0/32768.0;
break;
case AFS_8G:
aRes = 8.0/32768.0;
break;
case AFS_16G:
aRes = 16.0/32768.0;
break;
}
}


void readAccelData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z accel register data stored here
readBytes(MPU9250_ADDRESS, ACCEL_XOUT_H, 6, &rawData[0]); // Read the six raw data registers into data array
destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}


void readGyroData(int16_t * destination)
{
uint8_t rawData[6]; // x/y/z gyro register data stored here
readBytes(MPU9250_ADDRESS, GYRO_XOUT_H, 6, &rawData[0]); // Read the six raw data registers sequentially into data array
destination[0] = ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[2] << 8) | rawData[3] ;
destination[2] = ((int16_t)rawData[4] << 8) | rawData[5] ;
}

void readMagData(int16_t * destination)
{
uint8_t rawData[7]; // x/y/z gyro register data, ST2 register stored here, must read ST2 at end of data acquisition
if(readByte(AK8963_ADDRESS, AK8963_ST1) & 0x01) { // wait for magnetometer data ready bit to be set
readBytes(AK8963_ADDRESS, AK8963_XOUT_L, 7, &rawData[0]); // Read the six raw data and ST2 registers sequentially into data array
uint8_t c = rawData[6]; // End data read by reading ST2 register
if(!(c & 0x08)) { // Check if magnetic sensor overflow set, if not then report data
destination[0] = ((int16_t)rawData[1] << 8) | rawData[0] ; // Turn the MSB and LSB into a signed 16-bit value
destination[1] = ((int16_t)rawData[3] << 8) | rawData[2] ; // Data stored as little Endian
destination[2] = ((int16_t)rawData[5] << 8) | rawData[4] ;
}
}
}

int16_t readTempData()
{
uint8_t rawData[2]; // x/y/z gyro register data stored here
readBytes(MPU9250_ADDRESS, TEMP_OUT_H, 2, &rawData[0]); // Read the two raw data registers sequentially into data array
return ((int16_t)rawData[0] << 8) | rawData[1] ; // Turn the MSB and LSB into a 16-bit value
}

void initAK8963(float * destination)
{
// First extract the factory calibration for each magnetometer axis
uint8_t rawData[3]; // x/y/z gyro calibration data stored here
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
delay(10);
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x0F); // Enter Fuse ROM access mode
delay(10);
readBytes(AK8963_ADDRESS, AK8963_ASAX, 3, &rawData[0]); // Read the x-, y-, and z-axis calibration values
destination[0] = (float)(rawData[0] - 128)/256. + 1.; // Return x-axis sensitivity adjustment values, etc.
destination[1] = (float)(rawData[1] - 128)/256. + 1.;
destination[2] = (float)(rawData[2] - 128)/256. + 1.;
writeByte(AK8963_ADDRESS, AK8963_CNTL, 0x00); // Power down magnetometer
delay(10);
// Configure the magnetometer for continuous read and highest resolution
// set Mscale bit 4 to 1 (0) to enable 16 (14) bit resolution in CNTL register,
// and enable continuous mode data acquisition Mmode (bits [3:0]), 0010 for 8 Hz and 0110 for 100 Hz sample rates
writeByte(AK8963_ADDRESS, AK8963_CNTL, Mscale << 4 | Mmode); // Set magnetometer data resolution and sample ODR
delay(10);
}


void initMPU9250()
{
// wake up device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Clear sleep mode bit (6), enable all sensors
delay(100); // Wait for all registers to reset

// get stable time source
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01); // Auto select clock source to be PLL gyroscope reference if ready else
delay(200);

// Configure Gyro and Thermometer
// Disable FSYNC and set thermometer and gyro bandwidth to 41 and 42 Hz, respectively;
// minimum delay time for this setting is 5.9 ms, which means sensor fusion update rates cannot
// be higher than 1 / 0.0059 = 170 Hz
// DLPF_CFG = bits 2:0 = 011; this limits the sample rate to 1000 Hz for both
// With the MPU9250, it is possible to get gyro sample rates of 32 kHz (!), 8 kHz, or 1 kHz
writeByte(MPU9250_ADDRESS, CONFIG, 0x03);

// Set sample rate = gyroscope output rate/(1 + SMPLRT_DIV)
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x04); // Use a 200 Hz rate; a rate consistent with the filter update rate
// determined inset in CONFIG above

// Set gyroscope full scale range
// Range selects FS_SEL and AFS_SEL are 0 - 3, so 2-bit values are left-shifted into positions 4:3
uint8_t c = readByte(MPU9250_ADDRESS, GYRO_CONFIG);
// writeRegister(GYRO_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x02); // Clear Fchoice bits [1:0]
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, c | Gscale << 3); // Set full scale range for the gyro
// writeRegister(GYRO_CONFIG, c | 0x00); // Set Fchoice for the gyro to 11 by writing its inverse to bits 1:0 of GYRO_CONFIG

// Set accelerometer full-scale range configuration
c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG);
// writeRegister(ACCEL_CONFIG, c & ~0xE0); // Clear self-test bits [7:5]
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c & ~0x18); // Clear AFS bits [4:3]
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, c | Ascale << 3); // Set full scale range for the accelerometer

// Set accelerometer sample rate configuration
// It is possible to get a 4 kHz sample rate from the accelerometer by choosing 1 for
// accel_fchoice_b bit [3]; in this case the bandwidth is 1.13 kHz
c = readByte(MPU9250_ADDRESS, ACCEL_CONFIG2);
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c & ~0x0F); // Clear accel_fchoice_b (bit 3) and A_DLPFG (bits [2:0])
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG2, c | 0x03); // Set accelerometer rate to 1 kHz and bandwidth to 41 Hz

// The accelerometer, gyro, and thermometer are set to 1 kHz sample rates,
// but all these rates are further reduced by a factor of 5 to 200 Hz because of the SMPLRT_DIV setting

// Configure Interrupts and Bypass Enable
// Set interrupt pin active high, push-pull, hold interrupt pin level HIGH until interrupt cleared,
// clear on read of INT_STATUS, and enable I2C_BYPASS_EN so additional chips
// can join the I2C bus and all can be controlled by the Arduino as master
writeByte(MPU9250_ADDRESS, INT_PIN_CFG, 0x22);
writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x01); // Enable data ready (bit 0) interrupt
delay(100);
}


// Function which accumulates gyro and accelerometer data after device initialization. It calculates the average
// of the at-rest readings and then loads the resulting offsets into accelerometer and gyro bias registers.
void calibrateMPU9250(float * dest1, float * dest2)
{
uint8_t data[12]; // data array to hold accelerometer and gyro x, y, z, data
uint16_t ii, packet_count, fifo_count;
int32_t gyro_bias[3] = {0, 0, 0}, accel_bias[3] = {0, 0, 0};

// reset device
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x80); // Write a one to bit 7 reset bit; toggle reset device
delay(100);

// get stable time source; Auto select clock source to be PLL gyroscope reference if ready
// else use the internal oscillator, bits 2:0 = 001
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x01);
writeByte(MPU9250_ADDRESS, PWR_MGMT_2, 0x00);
delay(200);

// Configure device for bias calculation
writeByte(MPU9250_ADDRESS, INT_ENABLE, 0x00); // Disable all interrupts
writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable FIFO
writeByte(MPU9250_ADDRESS, PWR_MGMT_1, 0x00); // Turn on internal clock source
writeByte(MPU9250_ADDRESS, I2C_MST_CTRL, 0x00); // Disable I2C master
writeByte(MPU9250_ADDRESS, USER_CTRL, 0x00); // Disable FIFO and I2C master modes
writeByte(MPU9250_ADDRESS, USER_CTRL, 0x0C); // Reset FIFO and DMP
delay(15);

// Configure MPU6050 gyro and accelerometer for bias calculation
writeByte(MPU9250_ADDRESS, CONFIG, 0x01); // Set low-pass filter to 188 Hz
writeByte(MPU9250_ADDRESS, SMPLRT_DIV, 0x00); // Set sample rate to 1 kHz
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0x00); // Set gyro full-scale to 250 degrees per second, maximum sensitivity
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0x00); // Set accelerometer full-scale to 2 g, maximum sensitivity

uint16_t gyrosensitivity = 131; // = 131 LSB/degrees/sec
uint16_t accelsensitivity = 16384; // = 16384 LSB/g

// Configure FIFO to capture accelerometer and gyro data for bias calculation
writeByte(MPU9250_ADDRESS, USER_CTRL, 0x40); // Enable FIFO
writeByte(MPU9250_ADDRESS, FIFO_EN, 0x78); // Enable gyro and accelerometer sensors for FIFO (max size 512 bytes in MPU-9150)
delay(40); // accumulate 40 samples in 40 milliseconds = 480 bytes

// At end of sample accumulation, turn off FIFO sensor read
writeByte(MPU9250_ADDRESS, FIFO_EN, 0x00); // Disable gyro and accelerometer sensors for FIFO
readBytes(MPU9250_ADDRESS, FIFO_COUNTH, 2, &data[0]); // read FIFO sample count
fifo_count = ((uint16_t)data[0] << 8) | data[1];
packet_count = fifo_count/12;// How many sets of full gyro and accelerometer data for averaging

for (ii = 0; ii < packet_count; ii++) {
int16_t accel_temp[3] = {0, 0, 0}, gyro_temp[3] = {0, 0, 0};
readBytes(MPU9250_ADDRESS, FIFO_R_W, 12, &data[0]); // read data for averaging
accel_temp[0] = (int16_t) (((int16_t)data[0] << 8) | data[1] ) ; // Form signed 16-bit integer for each sample in FIFO
accel_temp[1] = (int16_t) (((int16_t)data[2] << 8) | data[3] ) ;
accel_temp[2] = (int16_t) (((int16_t)data[4] << 8) | data[5] ) ;
gyro_temp[0] = (int16_t) (((int16_t)data[6] << 8) | data[7] ) ;
gyro_temp[1] = (int16_t) (((int16_t)data[8] << 8) | data[9] ) ;
gyro_temp[2] = (int16_t) (((int16_t)data[10] << 8) | data[11]) ;

accel_bias[0] += (int32_t) accel_temp[0]; // Sum individual signed 16-bit biases to get accumulated signed 32-bit biases
accel_bias[1] += (int32_t) accel_temp[1];
accel_bias[2] += (int32_t) accel_temp[2];
gyro_bias[0] += (int32_t) gyro_temp[0];
gyro_bias[1] += (int32_t) gyro_temp[1];
gyro_bias[2] += (int32_t) gyro_temp[2];

}
accel_bias[0] /= (int32_t) packet_count; // Normalize sums to get average count biases
accel_bias[1] /= (int32_t) packet_count;
accel_bias[2] /= (int32_t) packet_count;
gyro_bias[0] /= (int32_t) packet_count;
gyro_bias[1] /= (int32_t) packet_count;
gyro_bias[2] /= (int32_t) packet_count;

if(accel_bias[2] > 0L) {accel_bias[2] -= (int32_t) accelsensitivity;} // Remove gravity from the z-axis accelerometer bias calculation
else {accel_bias[2] += (int32_t) accelsensitivity;}

// Construct the gyro biases for push to the hardware gyro bias registers, which are reset to zero upon device startup
data[0] = (-gyro_bias[0]/4 >> 8) & 0xFF; // Divide by 4 to get 32.9 LSB per deg/s to conform to expected bias input format
data[1] = (-gyro_bias[0]/4) & 0xFF; // Biases are additive, so change sign on calculated average gyro biases
data[2] = (-gyro_bias[1]/4 >> 8) & 0xFF;
data[3] = (-gyro_bias[1]/4) & 0xFF;
data[4] = (-gyro_bias[2]/4 >> 8) & 0xFF;
data[5] = (-gyro_bias[2]/4) & 0xFF;

// Push gyro biases to hardware registers
writeByte(MPU9250_ADDRESS, XG_OFFSET_H, data[0]);
writeByte(MPU9250_ADDRESS, XG_OFFSET_L, data[1]);
writeByte(MPU9250_ADDRESS, YG_OFFSET_H, data[2]);
writeByte(MPU9250_ADDRESS, YG_OFFSET_L, data[3]);
writeByte(MPU9250_ADDRESS, ZG_OFFSET_H, data[4]);
writeByte(MPU9250_ADDRESS, ZG_OFFSET_L, data[5]);

// Output scaled gyro biases for display in the main program
dest1[0] = (float) gyro_bias[0]/(float) gyrosensitivity;
dest1[1] = (float) gyro_bias[1]/(float) gyrosensitivity;
dest1[2] = (float) gyro_bias[2]/(float) gyrosensitivity;

// Construct the accelerometer biases for push to the hardware accelerometer bias registers. These registers contain
// factory trim values which must be added to the calculated accelerometer biases; on boot up these registers will hold
// non-zero values. In addition, bit 0 of the lower byte must be preserved since it is used for temperature
// compensation calculations. Accelerometer bias registers expect bias input as 2048 LSB per g, so that
// the accelerometer biases calculated above must be divided by 8.

int32_t accel_bias_reg[3] = {0, 0, 0}; // A place to hold the factory accelerometer trim biases
readBytes(MPU9250_ADDRESS, XA_OFFSET_H, 2, &data[0]); // Read factory accelerometer trim values
accel_bias_reg[0] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
readBytes(MPU9250_ADDRESS, YA_OFFSET_H, 2, &data[0]);
accel_bias_reg[1] = (int32_t) (((int16_t)data[0] << 8) | data[1]);
readBytes(MPU9250_ADDRESS, ZA_OFFSET_H, 2, &data[0]);
accel_bias_reg[2] = (int32_t) (((int16_t)data[0] << 8) | data[1]);

uint32_t mask = 1uL; // Define mask for temperature compensation bit 0 of lower byte of accelerometer bias registers
uint8_t mask_bit[3] = {0, 0, 0}; // Define array to hold mask bit for each accelerometer bias axis

for(ii = 0; ii < 3; ii++) {
if((accel_bias_reg[ii] & mask)) mask_bit[ii] = 0x01; // If temperature compensation bit is set, record that fact in mask_bit
}

// Construct total accelerometer bias, including calculated average accelerometer bias from above
accel_bias_reg[0] -= (accel_bias[0]/8); // Subtract calculated averaged accelerometer bias scaled to 2048 LSB/g (16 g full scale)
accel_bias_reg[1] -= (accel_bias[1]/8);
accel_bias_reg[2] -= (accel_bias[2]/8);

data[0] = (accel_bias_reg[0] >> 8) & 0xFF;
data[1] = (accel_bias_reg[0]) & 0xFF;
data[1] = data[1] | mask_bit[0]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[2] = (accel_bias_reg[1] >> 8) & 0xFF;
data[3] = (accel_bias_reg[1]) & 0xFF;
data[3] = data[3] | mask_bit[1]; // preserve temperature compensation bit when writing back to accelerometer bias registers
data[4] = (accel_bias_reg[2] >> 8) & 0xFF;
data[5] = (accel_bias_reg[2]) & 0xFF;
data[5] = data[5] | mask_bit[2]; // preserve temperature compensation bit when writing back to accelerometer bias registers

// Apparently this is not working for the acceleration biases in the MPU-9250
// Are we handling the temperature correction bit properly?
// Push accelerometer biases to hardware registers
// writeByte(MPU9250_ADDRESS, XA_OFFSET_H, data[0]);
// writeByte(MPU9250_ADDRESS, XA_OFFSET_L, data[1]);
// writeByte(MPU9250_ADDRESS, YA_OFFSET_H, data[2]);
// writeByte(MPU9250_ADDRESS, YA_OFFSET_L, data[3]);
// writeByte(MPU9250_ADDRESS, ZA_OFFSET_H, data[4]);
// writeByte(MPU9250_ADDRESS, ZA_OFFSET_L, data[5]);

// Output scaled accelerometer biases for display in the main program
dest2[0] = (float)accel_bias[0]/(float)accelsensitivity;
dest2[1] = (float)accel_bias[1]/(float)accelsensitivity;
dest2[2] = (float)accel_bias[2]/(float)accelsensitivity;
}

// Accelerometer and gyroscope self test; check calibration wrt factory settings
void MPU9250SelfTest(float * destination) // Should return percent deviation from factory trim values, +/- 14 or less deviation is a pass
{
uint8_t rawData[4];
uint8_t selfTest[6];
float factoryTrim[6];

// Configure the accelerometer for self-test
writeByte(MPU9250_ADDRESS, ACCEL_CONFIG, 0xF0); // Enable self test on all three axes and set accelerometer range to +/- 8 g
writeByte(MPU9250_ADDRESS, GYRO_CONFIG, 0xE0); // Enable self test on all three axes and set gyro range to +/- 250 degrees/s
delay(250); // Delay a while to let the device execute the self-test
rawData[0] = readByte(MPU9250_ADDRESS, SELF_TEST_X_ACCEL); // X-axis self-test results
rawData[1] = readByte(MPU9250_ADDRESS, SELF_TEST_Y_ACCEL); // Y-axis self-test results
rawData[2] = readByte(MPU9250_ADDRESS, SELF_TEST_Z_ACCEL); // Z-axis self-test results
// Extract the acceleration test results first
selfTest[0] = (rawData[0] >> 3) | (rawData[3] & 0x30) >> 4 ; // XA_TEST result is a five-bit unsigned integer
selfTest[1] = (rawData[1] >> 3) | (rawData[3] & 0x0C) >> 4 ; // YA_TEST result is a five-bit unsigned integer
selfTest[2] = (rawData[2] >> 3) | (rawData[3] & 0x03) >> 4 ; // ZA_TEST result is a five-bit unsigned integer
// Extract the gyration test results first
selfTest[3] = rawData[0] & 0xD0 ; // XG_TEST result is a five-bit unsigned integer
selfTest[4] = rawData[1] & 0xD0 ; // YG_TEST result is a five-bit unsigned integer
selfTest[5] = rawData[2] & 0xD0 ; // ZG_TEST result is a five-bit unsigned integer
// Process results to allow final comparison with factory set values
factoryTrim[0] = (4096.0*0.34)*(pow( (0.92/0.34) , (((float)selfTest[0] - 1.0)/30.0))); // FT[Xa] factory trim calculation
factoryTrim[1] = (4096.0*0.34)*(pow( (0.92/0.34) , (((float)selfTest[1] - 1.0)/30.0))); // FT[Ya] factory trim calculation
factoryTrim[2] = (4096.0*0.34)*(pow( (0.92/0.34) , (((float)selfTest[2] - 1.0)/30.0))); // FT[Za] factory trim calculation
factoryTrim[3] = ( 25.0*131.0)*(pow( 1.046 , ((float)selfTest[3] - 1.0) )); // FT[Xg] factory trim calculation
factoryTrim[4] = (-25.0*131.0)*(pow( 1.046 , ((float)selfTest[4] - 1.0) )); // FT[Yg] factory trim calculation
factoryTrim[5] = ( 25.0*131.0)*(pow( 1.046 , ((float)selfTest[5] - 1.0) )); // FT[Zg] factory trim calculation

// Output self-test results and factory trim calculation if desired
// Serial.println(selfTest[0]); Serial.println(selfTest[1]); Serial.println(selfTest[2]);
// Serial.println(selfTest[3]); Serial.println(selfTest[4]); Serial.println(selfTest[5]);
// Serial.println(factoryTrim[0]); Serial.println(factoryTrim[1]); Serial.println(factoryTrim[2]);
// Serial.println(factoryTrim[3]); Serial.println(factoryTrim[4]); Serial.println(factoryTrim[5]);

// Report results as a ratio of (STR - FT)/FT; the change from Factory Trim of the Self-Test Response
// To get to percent, must multiply by 100 and subtract result from 100
for (int i = 0; i < 6; i++) {
destination[i] = 100.0 + 100.0*((float)selfTest[i] - factoryTrim[i])/factoryTrim[i]; // Report percent differences
}

}



void writeByte(uint8_t address, uint8_t subAddress, uint8_t data)
{
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.write(data); // Put data in Tx buffer
Wire.endTransmission(); // Send the Tx buffer
}

uint8_t readByte(uint8_t address, uint8_t subAddress)
{
uint8_t data; // `data` will store the register data
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
// Wire.requestFrom(address, 1); // Read one byte from slave register address
Wire.requestFrom(address, (size_t) 1); // Read one byte from slave register address
data = Wire.read(); // Fill Rx buffer with result
return data; // Return data read from slave register
}

void readBytes(uint8_t address, uint8_t subAddress, uint8_t count, uint8_t * dest)
{
Wire.beginTransmission(address); // Initialize the Tx buffer
Wire.write(subAddress); // Put slave register address in Tx buffer
Wire.endTransmission(I2C_NOSTOP); // Send the Tx buffer, but send a restart to keep connection alive
// Wire.endTransmission(false); // Send the Tx buffer, but send a restart to keep connection alive
uint8_t i = 0;
// Wire.requestFrom(address, count); // Read bytes from slave register address
Wire.requestFrom(address, (size_t) count); // Read bytes from slave register address
while (Wire.available()) {
dest[i++] = Wire.read(); } // Put read results in the Rx buffer
}



Which requires this library function:



// Implementation of Sebastian Madgwick's "...efficient orientation filter for... inertial/magnetic sensor arrays"
// (see http://www.x-io.co.uk/category/open-source/ for examples and more details)
// which fuses acceleration, rotation rate, and magnetic moments to produce a quaternion-based estimate of absolute
// device orientation -- which can be converted to yaw, pitch, and roll. Useful for stabilizing quadcopters, etc.
// The performance of the orientation filter is at least as good as conventional Kalman-based filtering algorithms
// but is much less computationally intensive---it can be performed on a 3.3 V Pro Mini operating at 8 MHz!
void MadgwickQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, _2bx, _2bz;
float s1, s2, s3, s4;
float qDot1, qDot2, qDot3, qDot4;

// Auxiliary variables to avoid repeated arithmetic
float _2q1mx;
float _2q1my;
float _2q1mz;
float _2q2mx;
float _4bx;
float _4bz;
float _2q1 = 2.0f * q1;
float _2q2 = 2.0f * q2;
float _2q3 = 2.0f * q3;
float _2q4 = 2.0f * q4;
float _2q1q3 = 2.0f * q1 * q3;
float _2q3q4 = 2.0f * q3 * q4;
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;

// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
ax *= norm;
ay *= norm;
az *= norm;

// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f/norm;
mx *= norm;
my *= norm;
mz *= norm;

// Reference direction of Earth's magnetic field
_2q1mx = 2.0f * q1 * mx;
_2q1my = 2.0f * q1 * my;
_2q1mz = 2.0f * q1 * mz;
_2q2mx = 2.0f * q2 * mx;
hx = mx * q1q1 - _2q1my * q4 + _2q1mz * q3 + mx * q2q2 + _2q2 * my * q3 + _2q2 * mz * q4 - mx * q3q3 - mx * q4q4;
hy = _2q1mx * q4 + my * q1q1 - _2q1mz * q2 + _2q2mx * q3 - my * q2q2 + my * q3q3 + _2q3 * mz * q4 - my * q4q4;
_2bx = sqrt(hx * hx + hy * hy);
_2bz = -_2q1mx * q3 + _2q1my * q2 + mz * q1q1 + _2q2mx * q4 - mz * q2q2 + _2q3 * my * q4 - mz * q3q3 + mz * q4q4;
_4bx = 2.0f * _2bx;
_4bz = 2.0f * _2bz;

// Gradient decent algorithm corrective step
s1 = -_2q3 * (2.0f * q2q4 - _2q1q3 - ax) + _2q2 * (2.0f * q1q2 + _2q3q4 - ay) - _2bz * q3 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q4 + _2bz * q2) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q3 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s2 = _2q4 * (2.0f * q2q4 - _2q1q3 - ax) + _2q1 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q2 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + _2bz * q4 * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q3 + _2bz * q1) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q4 - _4bz * q2) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s3 = -_2q1 * (2.0f * q2q4 - _2q1q3 - ax) + _2q4 * (2.0f * q1q2 + _2q3q4 - ay) - 4.0f * q3 * (1.0f - 2.0f * q2q2 - 2.0f * q3q3 - az) + (-_4bx * q3 - _2bz * q1) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (_2bx * q2 + _2bz * q4) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + (_2bx * q1 - _4bz * q3) * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
s4 = _2q2 * (2.0f * q2q4 - _2q1q3 - ax) + _2q3 * (2.0f * q1q2 + _2q3q4 - ay) + (-_4bx * q4 + _2bz * q2) * (_2bx * (0.5f - q3q3 - q4q4) + _2bz * (q2q4 - q1q3) - mx) + (-_2bx * q1 + _2bz * q3) * (_2bx * (q2q3 - q1q4) + _2bz * (q1q2 + q3q4) - my) + _2bx * q2 * (_2bx * (q1q3 + q2q4) + _2bz * (0.5f - q2q2 - q3q3) - mz);
norm = sqrt(s1 * s1 + s2 * s2 + s3 * s3 + s4 * s4); // normalise step magnitude
norm = 1.0f/norm;
s1 *= norm;
s2 *= norm;
s3 *= norm;
s4 *= norm;

// Compute rate of change of quaternion
qDot1 = 0.5f * (-q2 * gx - q3 * gy - q4 * gz) - beta * s1;
qDot2 = 0.5f * (q1 * gx + q3 * gz - q4 * gy) - beta * s2;
qDot3 = 0.5f * (q1 * gy - q2 * gz + q4 * gx) - beta * s3;
qDot4 = 0.5f * (q1 * gz + q2 * gy - q3 * gx) - beta * s4;

// Integrate to yield quaternion
q1 += qDot1 * deltat;
q2 += qDot2 * deltat;
q3 += qDot3 * deltat;
q4 += qDot4 * deltat;
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4); // normalise quaternion
norm = 1.0f/norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;

}



// Similar to Madgwick scheme but uses proportional and integral filtering on the error between estimated reference vectors and
// measured ones.
void MahonyQuaternionUpdate(float ax, float ay, float az, float gx, float gy, float gz, float mx, float my, float mz)
{
float q1 = q[0], q2 = q[1], q3 = q[2], q4 = q[3]; // short name local variable for readability
float norm;
float hx, hy, bx, bz;
float vx, vy, vz, wx, wy, wz;
float ex, ey, ez;
float pa, pb, pc;

// Auxiliary variables to avoid repeated arithmetic
float q1q1 = q1 * q1;
float q1q2 = q1 * q2;
float q1q3 = q1 * q3;
float q1q4 = q1 * q4;
float q2q2 = q2 * q2;
float q2q3 = q2 * q3;
float q2q4 = q2 * q4;
float q3q3 = q3 * q3;
float q3q4 = q3 * q4;
float q4q4 = q4 * q4;

// Normalise accelerometer measurement
norm = sqrt(ax * ax + ay * ay + az * az);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
ax *= norm;
ay *= norm;
az *= norm;

// Normalise magnetometer measurement
norm = sqrt(mx * mx + my * my + mz * mz);
if (norm == 0.0f) return; // handle NaN
norm = 1.0f / norm; // use reciprocal for division
mx *= norm;
my *= norm;
mz *= norm;

// Reference direction of Earth's magnetic field
hx = 2.0f * mx * (0.5f - q3q3 - q4q4) + 2.0f * my * (q2q3 - q1q4) + 2.0f * mz * (q2q4 + q1q3);
hy = 2.0f * mx * (q2q3 + q1q4) + 2.0f * my * (0.5f - q2q2 - q4q4) + 2.0f * mz * (q3q4 - q1q2);
bx = sqrt((hx * hx) + (hy * hy));
bz = 2.0f * mx * (q2q4 - q1q3) + 2.0f * my * (q3q4 + q1q2) + 2.0f * mz * (0.5f - q2q2 - q3q3);

// Estimated direction of gravity and magnetic field
vx = 2.0f * (q2q4 - q1q3);
vy = 2.0f * (q1q2 + q3q4);
vz = q1q1 - q2q2 - q3q3 + q4q4;
wx = 2.0f * bx * (0.5f - q3q3 - q4q4) + 2.0f * bz * (q2q4 - q1q3);
wy = 2.0f * bx * (q2q3 - q1q4) + 2.0f * bz * (q1q2 + q3q4);
wz = 2.0f * bx * (q1q3 + q2q4) + 2.0f * bz * (0.5f - q2q2 - q3q3);

// Error is cross product between estimated direction and measured direction of gravity
ex = (ay * vz - az * vy) + (my * wz - mz * wy);
ey = (az * vx - ax * vz) + (mz * wx - mx * wz);
ez = (ax * vy - ay * vx) + (mx * wy - my * wx);
if (Ki > 0.0f)
{
eInt[0] += ex; // accumulate integral error
eInt[1] += ey;
eInt[2] += ez;
}
else
{
eInt[0] = 0.0f; // prevent integral wind up
eInt[1] = 0.0f;
eInt[2] = 0.0f;
}

// Apply feedback terms
gx = gx + Kp * ex + Ki * eInt[0];
gy = gy + Kp * ey + Ki * eInt[1];
gz = gz + Kp * ez + Ki * eInt[2];

// Integrate rate of change of quaternion
pa = q2;
pb = q3;
pc = q4;
q1 = q1 + (-q2 * gx - q3 * gy - q4 * gz) * (0.5f * deltat);
q2 = pa + (q1 * gx + pb * gz - pc * gy) * (0.5f * deltat);
q3 = pb + (q1 * gy - pa * gz + pc * gx) * (0.5f * deltat);
q4 = pc + (q1 * gz + pa * gy - pb * gx) * (0.5f * deltat);

// Normalise quaternion
norm = sqrt(q1 * q1 + q2 * q2 + q3 * q3 + q4 * q4);
norm = 1.0f / norm;
q[0] = q1 * norm;
q[1] = q2 * norm;
q[2] = q3 * norm;
q[3] = q4 * norm;

}

Nantonos
07-05-2014, 12:35 AM
onehorse, where did you install the libraries needed for that sketch? If you put them inside your sketches/libraries then they will be available for all installs of Arduino (assuming they all use the same sketch location). On the other hand if you put them in the Arduino/TeensyDuino libraries folder, you need to do that for each new version you install.

I find its best to install Teensyduino with the "all" libraries option so you have all the standard ones, and put any others under sketches/libraries

onehorse
07-05-2014, 05:54 AM
I just migrated to a new laptop (Windows 7) and I've been having some trouble getting everything set up the way I like it and working as it used to. This is one such case. I can see the i2c_t3.h library in the Sketch/Import Library pull down in the contributed category. When I installed the 1.20 version of teensyduino I did indeed check the 'all' box. I just checked on my old laptop; the sketch above works fine with the i2c_t3 library under the old version 1.18; I never upgraded to 1.19. It's more than likely I did something stupid when I installed version 1.20. Still, can anyone confirm that i2c_t3.h works with version 1.20?

onehorse
07-05-2014, 06:59 AM
I reinstalled Arduino v.1.05-r2 and Teensyduino 1.20 and i2c_t3.v6b, in that order, and I can now compile usin i2c_t3 without complaint. I think I inadvertently downloaded the first i2c_t3 file in the list the first time, which turns out to be the earliest version, which apparently isn't compatible with the latest syntax. Pilot error, but at least I'm consistent!

PaulStoffregen
07-05-2014, 07:22 AM
I noticed that now there are a greater variety of clock speeds available. Does 'no USB' mean that one cannot use the USB in a program at these low clock speeds


Yes. The USB port does not function when the clock is below 24 MHz.



or that one cannot enter a new program through the USB at these low clock speed settings?


The automatic reboot, when you click Upload in Arduino, also does not work, because the USB isn't active.

You will need to press the pushbutton to initiate update. After you release the pushbutton, the Mini54 takes control and reconfigures the MK20 to 24 MHz for the duration of the upload.



Could you comment on this increase in clock speed setting choices? Thanks.

For about half a year after releasing Teensy 3.1, the software only supported 24, 48 and 96 MHz, even though the chip is rated by Freescale for up to 72 MHz.

Inside the chip, the are 3 main clocks, for the CPU, peripherals and flash memory. Originally, Teensyduino used either 48 MHz when the CPU was 48 or 96 MHz, or 24 MHz, for peripherals. Most peripherals have some sort of prescaler in powers of 2, so code supporting 48 MHz and half of that speed was fairly simple.

72 MHz CPU speed resulted in a 36 MHz peripheral clock. I suspect many less ambitious software developers would have simply used 24 MHz instead of 36 MHz, but not me. Instead, I waited several months, so when I did support 72 MHz, I could spend some time to really work on editing the many places that depend on this clock. I usually prefer to spend more time to really do something well...

People also started to play with hacking even higher overclocking, and even slower clocks for low power applications. While editing the code, I expanded the list from only 2 peripheral speeds, 48 & 24, to 8 speeds: 60, 56, 48, 36, 24, 16, 8, 4, 2 MHz. The idea is that there are many places to edit to add any new peripheral speed, so I wanted to expand the list to a wide range that will (hopefully) cover all the speeds Teensyduino will support for quite a long time.

For extremely low power, the 2 MHz speed is special. The chip has a VLPR mode where the hardware internally plays tricks with the power supply voltage, which dramatically reduces the current consumed, but it only works up to 2 MHz. That's the reason why the speeds go all the way down to 2 MHz.

onehorse
07-05-2014, 07:39 AM
Thanks for the detailed reply. I am particularly interested in the VLP applications of the Teensy for the motion sensing and BLE add-ons I am designing. This new (to me) capability keeps Teensy in the running for such VLP applications against other worthy competitors like the STM32F401 ARM microprocessor I have been playing with.

Edit: There must be something more to running at 2 MHz; I just tried setting the 2 MHz speed on the sketch above and encountered this error:

C:\Users\Pesky Products\Documents\Arduino\libraries\i2c_t3\i2c_t3 .cpp:263:10: error: #error "F_BUS must be 48 MHz or 24 MHz"

Is the clock speed hardwired into the i2C_t3 library? I have run this sketch with this i2c_t3 library at 24, 48, and 96 MHz. What's the trick to running with the lower clock speeds?

3400tZ
07-08-2014, 02:41 AM
I've been following the thread related with this change "Fix ARM toolchain linker & zlib issue (thanks to Andrew Kroll)", so with 1.20 I was expecting my sketch to be much smaller (both flash and RAM), but it haven't seems to make a difference at all, it's still compiling all the usb_* classes that I don't use, the audio class, the ipaddress class, etc etc. Does I'm missing something ? :)

PaulStoffregen
07-08-2014, 04:46 AM
so with 1.20 I was expecting my sketch to be much smaller (both flash and RAM)

Please post a specific program, with the flash & ram size using both 1.19 and 1.20-rc1.

The toolchain still compiles all the same code. The difference is only what the linker keeps in your final executable image.

onehorse
07-08-2014, 05:07 AM
I have re-read your post on clock speeds several times and I think my confusion has to do with the difference between clock speed and the speed of peripherals that are 1/2n multiples. Does the 2 MHz capability only refer to the ability to scale the 48 MHz clock speed down to 2 MHz to run the CAN bus or I2S or whatever? Is that what you mean by peripherals? Or can we somehow run the Teensy clock at 2 MHz? As I am sure you know, clock speed is important for low power applications where the current goes as something like 200 microAmp per MHz. So a 2 MHz clock speed capability for those applications that could afford the speed slowdown would save significant power. Sorry for the lame questions but I really would like to understand this better.

PaulStoffregen
07-08-2014, 07:21 AM
Or can we somehow run the Teensy clock at 2 MHz?


Yes, that's exactly what it does!

Set Tools > CPU Speed to 2 MHz and your Teensy 3.1 will run at only 2 MHz speed.

Really, it's that easy. You choose the speed in that menu and it changes to that speed when your uploaded code runs. It's kinda like magic.



As I am sure you know, clock speed is important for low power applications where the current goes as something like 200 microAmp per MHz. So a 2 MHz clock speed capability for those applications that could afford the speed slowdown would save significant power.


That's why so much work went into version 1.19 to make these extra slow speeds available.



but I really would like to understand this better.

Whatever you select in the CPU Speed menu causes a name to be #defined in the code. Use File > Preferences to show verbose info while compiling to make the exact compiler commands appear when you upload. The code uses that info to configure the hardware.

To understand the hardware, you should look at chapter 5 of the reference manual (http://www.pjrc.com/teensy/K20P64M72SF1RM.pdf). On page 156, you'll see this diagram:

2352
(click for full size)

This circuitry lets the chip run from a wide variety of speeds. The system osc on Teensy is always 16 MHz, but the PLL can multiply the frequency, and those OUTDIV circuits can divide it. There are MANY different clocks inside this chip, as you can see in the huge list of section 5.4, but the 3 main ones from from those 3 OUTDIV circuits. Section 5.5 explains some of the requirements of how those 3 clocks must be related to each other. Section 5.7 explains which things in the chip run from which clocks. You'll see almost everything runs from either the system clock or bus (peripheral) clock.

Another key piece of information is this list of clocks:

https://github.com/PaulStoffregen/cores/blob/master/teensy3/kinetis.h#L176

These are the actual clock speeds for all 3, for each of the options you can set in Tools > CPU Speed. This list is merely for documentation, so other code can know which speed is in use. The actual code which configures the hardware is here:

https://github.com/PaulStoffregen/cores/blob/master/teensy3/mk20dx128.c#L414

You can see there are many #ifdef checks, because the hardware is configured differently depending on whether the PLL is needed to increase the frequency, and of course the OUTDIV settings are different for each speed. The exact functionality of all those registers is documented in the reference manual. That PDF is huge, but you can use text search to find those names within the document to learn how each register works.

If you want to understand how it really works, rather than just take my word for the fact that menu really does control the speed, you're going to need to really read that reference manual documentation and the source code. I've written all I can here to point to you to the places to look.

onehorse
07-08-2014, 05:39 PM
Thanks for the patient explanation Paul, you've made it easy for me to learn what I want to know.
I'll study the reference manual so I can understand what you've done in the source code.
Next time I'll have a more intelligent question for you!

3400tZ
07-08-2014, 11:48 PM
Please post a specific program, with the flash & ram size using both 1.19 and 1.20-rc1.

The toolchain still compiles all the same code. The difference is only what the linker keeps in your final executable image.

Hi Paul, code is here (https://bitbucket.org/SebGiroux/diyecmgauge/src/447c1fc464b4ea50a464eef1f76b0640386a48dc/Firmware/?at=master).

I'm just a bit surprised because the flash / RAM usage output by the IDE are the same with 1.19 and 1.20:

Binary sketch size: 105,964 bytes (of a 262,144 byte maximum)
Estimated memory use: 15,492 bytes (of a 65,536 byte maximum)

I'm mainly surprised that my code use like 40% of the flash but I guess that's possible.

stevech
07-09-2014, 12:59 AM
In the ease of use department...
In addition to the change to boards.txt requested above,

Please change the window pop-up saying the selected board type in the IDE doesn't match the board type. This popup normally (windows 7) comes up hidden behind other windows. Confusing.
Also when this happens, the last line displayed in the IDE says "download complete" which adds to the confusion.

Nantonos
07-09-2014, 01:04 AM
I'm just a bit surprised because the flash / RAM usage output by the IDE are the same with 1.19 and 1.20
Its surprising they are exactly identical, since 1.20 puts the interrupt vector table in RAM so they should be slightly different in sizes even if the linker is unable to reduce size by not including stuff.

3400tZ
07-09-2014, 01:12 AM
Its surprising they are exactly identical, since 1.20 puts the interrupt vector table in RAM so they should be slightly different in sizes even if the linker is unable to reduce size by not including stuff.

Well, sorry, by the same, I mean similar, not EXACTLY the same.

1.19:

Binary sketch size: 106,248 bytes (of a 262,144 byte maximum)
Estimated memory use: 15,004 bytes (of a 65,536 byte maximum)

1.20:

Binary sketch size: 105,964 bytes (of a 262,144 byte maximum)
Estimated memory use: 15,492 bytes (of a 65,536 byte maximum)

PaulStoffregen
07-09-2014, 01:21 AM
Well, sorry, by the same, I mean similar, not EXACTLY the same.

If your program is actually using most of those stuff, like Serial1, Serial2 & Serial3, then the linker has to include it.

I'm wondering if the new serialEvent stuff is forcing the linker to keep those in the code unnecessarily? Honestly, reducing memory usage is a much lower priority for me than fixing the pin 33 issue and getting SPI transaction support working and patched into the widely used SPI-based libraries. If someone else (like Andrew Kroll - who's traditionally done the most of this) looks into the issue and suggests a fix, I'll give it a try. But I'm not going to divert my limited dev hours away from fixing pin 33 and SPI compatibility.

3400tZ
07-09-2014, 01:29 AM
Sure Paul, I'm not too worried about it, just thought I would still mention it :)

Nantonos
07-09-2014, 03:22 AM
Well, sorry, by the same, I mean similar, not EXACTLY the same.
:) My reason for asking - if they were exactly the same, my next question would be to check you didn't accidentally compile twice with the same version.

I'm seeing variable differences depending on libraries used. An SSD1306 example compiled noticeably smaller, probably because the library mixes up SPI and I2C but only actually uses one of them. Other code is, as you noted, pretty similar.

teachop
07-12-2014, 10:47 AM
The recommended change to using 16MHz XTAL source for CAN0 peripheral is merged into master. If you need anything for pulling it into 1.20 let me know.
https://github.com/teachop/FlexCAN_Library

P.S. Paul I had a question here:
http://forum.pjrc.com/threads/24720-Teensy-3-1-and-CAN-Bus?p=50777&viewfull=1#post50777

Barney
07-14-2014, 06:12 AM
Dear Paul,
1.20 Release Candidate #1



#define FTM_CONF_BDMMODE (((n) & 3) << 6) // Behavior when in debug mode
#define FTM_CONF_NUMTOF (((n) & 31) << 0) // ratio of counter overflows


must be


#define FTM_CONF_BDMMODE(n) (((n) & 3) << 6) // Behavior when in debug mode
#define FTM_CONF_NUMTOF(n) (((n) & 31) << 0) // ratio of counter overflows


why do you don't use _MASK extension
like -> FTM_MODE_FTMEN_MASK;
i must rewritten some example Code!

Barney

Alkorin
07-14-2014, 04:39 PM
I've at home a Teensy3.0 and I use it with OctoWS2811 and 256leds/strip.
With that number of leds a free byte of ram is a free byte !

I don't care to have USB serial, I just need to be able to flash the teensy.
I'll be nice to have an option to disable it.

With this small (ugly?) patch which add an option : USB_NONE, I save ~1.2kB of ram.


index 248cb1e..8508e85 100644
--- a/hardware/teensy/boards.txt
+++ b/hardware/teensy/boards.txt
@@ -146,6 +146,7 @@ teensy3.build.option3=-D__MK20DX128__
teensy3.build.option4=-DTEENSYDUINO=120
teensy3.build.cppoption1=-fno-rtti
teensy3.build.linkoption1=-mthumb
+teensy3.build.linkoption2=-Wl,-Map,map.out
teensy3.build.additionalobject1=-larm_cortexM4l_math
teensy3.build.linkscript=mk20dx128.ld
teensy3.build.architecture=arm-none-eabi
@@ -162,6 +163,8 @@ teensy3.build.dependency=true
teensy3.build.time_t=true
#teensy3.build.linker_relaxation=true
teensy3.build.post_compile_script=teensy_post_comp ile
+teensy3.menu.usb.none.name=None
+teensy3.menu.usb.none.build.define0=-DUSB_NONE
teensy3.menu.usb.serial.name=Serial
teensy3.menu.usb.serial.build.define0=-DUSB_SERIAL
teensy3.menu.usb.hid.name=Keyboard + Mouse + Joystick
diff --git a/hardware/teensy/cores/teensy3/usb_desc.h b/hardware/teensy/cores/teensy3/usb_desc.h
index c3cf15c..f1240e6 100644
--- a/hardware/teensy/cores/teensy3/usb_desc.h
+++ b/hardware/teensy/cores/teensy3/usb_desc.h
@@ -83,7 +83,27 @@ let me know? http://forum.pjrc.com/forums/4-Suggestions-amp-Bug-Reports



-#if defined(USB_SERIAL)
+#if defined(USB_NONE)
+ #define VENDOR_ID 0x16C0
+ #define PRODUCT_ID 0x0483
+ #define MANUFACTURER_NAME {'T','e','e','n','s','y','d','u','i','n','o'}
+ #define MANUFACTURER_NAME_LEN 11
+ #define PRODUCT_NAME {'U','S','B',' ','F','l','a','s','h'}
+ #define PRODUCT_NAME_LEN 9
+ #define EP0_SIZE 8
+ #define NUM_ENDPOINTS 0
+ #define NUM_USB_BUFFERS 0
+ #define NUM_INTERFACE 2
+ #define CDC_STATUS_INTERFACE 0
+ #define CDC_DATA_INTERFACE 1
+ #define CDC_ACM_ENDPOINT 2
+ #define CDC_RX_ENDPOINT 3
+ #define CDC_TX_ENDPOINT 4
+ #define CDC_ACM_SIZE 16
+ #define CDC_RX_SIZE 64
+ #define CDC_TX_SIZE 64
+ #define CONFIG_DESC_SIZE (9+9+5+5+4+5+7+9+7+7)
+#elif defined(USB_SERIAL)
#define VENDOR_ID 0x16C0
#define PRODUCT_ID 0x0483
#define DEVICE_CLASS 2 // 2 = Communication Class
diff --git a/hardware/teensy/cores/teensy3/yield.cpp b/hardware/teensy/cores/teensy3/yield.cpp
index 55e7d29..16ae996 100644
--- a/hardware/teensy/cores/teensy3/yield.cpp
+++ b/hardware/teensy/cores/teensy3/yield.cpp
@@ -40,7 +40,9 @@ void yield(void)

if (running) return; // TODO: does this need to be atomic?
running = 1;
+#ifdef USB_SERIAL
if (Serial.available()) serialEvent();
+#endif
if (Serial1.available()) serialEvent1();
if (Serial2.available()) serialEvent2();
if (Serial3.available()) serialEvent3();


This patch should be improvable. I just tried to reduce the USB buffers to save RAM.

Same thing for VectorsRam, if we don't need to change the vector table, it's 256 bytes of ram wasted :/


diff --git a/hardware/teensy/cores/teensy3/mk20dx128.c b/hardware/teensy/cores/teensy3/mk20dx128.c
index d9a751f..04abe32 100644
--- a/hardware/teensy/cores/teensy3/mk20dx128.c
+++ b/hardware/teensy/cores/teensy3/mk20dx128.c
@@ -407,9 +407,13 @@ void ResetHandler(void)
while (dest < &_ebss) *dest++ = 0;

// default all interrupts to medium priority level
+#if defined(STATIC_VECTOR_TABLE)
+ SCB_VTOR = (uint32_t)_VectorsFlash; // use vector table in FLASH
+#else
for (i=0; i < NVIC_NUM_INTERRUPTS + 16; i++) _VectorsRam[i] = _VectorsFlash[i];
- for (i=0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_PRIORITY(i, 128);
SCB_VTOR = (uint32_t)_VectorsRam; // use vector table in RAM
+#endif
+ for (i=0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_PRIORITY(i, 128);

// hardware always starts in FEI mode
// C1[CLKS] bits are written to 00

Alkorin
07-14-2014, 06:32 PM
Anyway, FASTRUN is a great idea !

It gives a serious boost on my plasma-like effect on ILI9341 : 134.7ms / frame => 62.3 ms / frame (-53% ... more than x2 !)

I now run my plasma effect @16fps, tx !

awardblvr
07-14-2014, 09:40 PM
Tried to DL

Mac OS-X:
http://www.pjrc.com/teensy/td_120-rc1/teensyduino.dmg

Multiple times today... Something seems just not quite right. Are others able to DL this?

Thanks

-A

Nantonos
07-15-2014, 07:47 AM
Is there a simple example showing how to use FASTRUN?

xxxajk
07-21-2014, 08:15 AM
This is actually in-the-works :-) If you run at < 20MHz, it will screw up too. I need to see if Paul has fixed this issue yet. With NO USB you can even do your own USB stuff, which is what I need.

PaulStoffregen
07-21-2014, 02:33 PM
Anyway, FASTRUN is a great idea !

It gives a serious boost on my plasma-like effect on ILI9341 : 134.7ms / frame => 62.3 ms / frame (-53% ... more than x2 !)


Is this code published anywhere? I'd love to give it a try, if it's available.

xxxajk
09-01-2014, 02:48 AM
I have two bugs reported on github.
The first bug is actually a set of bugs.
port[abcde]_isr should be prefixed with __attribute__((weak)) to allow them to be overridden. https://github.com/PaulStoffregen/cores/issues/34

Second bug is that micros() can't be called from within an ISR because it will enable interrupts. https://github.com/PaulStoffregen/cores/issues/35
There are a lot of other timing functions that also depend on it. Any of them used within an ISR will enable IRQs, which is bad.

Nantonos
09-01-2014, 01:01 PM
Self bump because I would like to know how to use FASTRUN

Is there a simple example showing how to use FASTRUN?

RussNelson
09-03-2014, 08:16 PM
Tested with current 64-bit Ubuntu 14.04 against current 64-bit Arduino code. Solves a problem I was having with Teensy 3.1 where the USB serial ports weren't appearing. Well done!

shanevanj
09-16-2014, 12:09 PM
I am trying to get serial event() to work - this code works on AVR but not on teensy - any suggestions (or corrections :-) )





char tempBuffer[100] = {0};
const int MaxInput = 10;
elapsedMillis hBeat; // "waiting" starts at zero

// ---------------------------------------------------------------------------
void setup()
// ---------------------------------------------------------------------------
{
Serial.begin(115200);
while (!Serial); // wait for connection to terminal emulator
Serial.println("Enter something");
}

// ---------------------------------------------------------------------------
void loop()
// ---------------------------------------------------------------------------
{
if (hBeat >= 1000)
{
Serial.println(".");
hBeat = 0; // reset count
}
}

// ---------------------------------------------------------------------------
void serialEvent()
// ---------------------------------------------------------------------------
{
static unsigned int input_pos = 0;
//Serial.println("+"); // debug so we know this has been called
while (Serial.available())
{
char inByte = (char)Serial.read();
switch (inByte)
{
case '\n': // end of text
tempBuffer [input_pos] = 0; // terminating null byte
Serial.println(tempBuffer);
input_pos = 0;
break;

default:
// only allow alphabet and punctuation chrs
if (inByte >= 32 && inByte <= 126)
{
// keep adding if not full ... allow for terminating null byte
if (input_pos < (MaxInput - 1)) tempBuffer[input_pos++] = inByte;
} // end if((inByte ....
break;
}
}
}

KurtE
09-16-2014, 02:02 PM
What version of the Teensyduino are you running? I believe this has been added for the current beta 1.20rc2 (maybe rc1 as well).

froeber
09-16-2014, 02:41 PM
I have two bugs reported on github.
Second bug is that micros() can't be called from within an ISR because it will enable interrupts. https://github.com/PaulStoffregen/cores/issues/35
There are a lot of other timing functions that also depend on it. Any of them used within an ISR will enable IRQs, which is bad.

Hi, I saw this post looking through the release notes. I also saw Paul closed your issue seeming to say it wasn't broken.
This notion of nested interrupt locking has come up before in this forum. And there seems to be this notion that because
you can use interrupt priority with our ARM MCU you don't have to worry about the interrupt locking issue. But that's
just sort of wrong I think. As you point out, the problem comes up with nested critical sections. Much current Teensy/Arduino code uses __disable_interrupt and __enable_interrupt to protect a critical section. However, if a call
is made inside a critical section to code that tries to also implement a critical section the same way then the code
breaks because the nested __enable_interrupts call unconditionally enables the interrupts breaking the critical section.
So, for any critical sections that can potentially be nested I use the inline routines I wrote below instead. It's a bit
more overhead but than the unconditional routines but they work. And they have to be used as you also note.


static inline uint32_t sl_hw_lock(void) __attribute__((always_inline, unused));
static inline void sl_hw_unlock(uint32_t primask) __attribute__((always_inline,
unused));

/**
* Lock interrupts to start a critical section.
*
* When used with sl_hw_unlock() this function starts a critical section that
* works properly even if nested in another critical section because it reads
* the PRIMASK value so it can be restored.
*
* @return the priority mask (PRIMASK) register value upon entry
*/
static inline uint32_t
sl_hw_lock(void)
{
uint32_t primask;

asm volatile("mrs %0, primask\n" : "=r" (primask)::);
__disable_irq();
return primask;
}

/**
* Unlock interrupts to end a critical section.
*
* When used with sl_hw_lock() this function ends a critical section that works
* properly even if nested in another critical section because it uses the
* previous interrupt locking state (defined by PRIMASK) to selectively unlock
* interrupts.
*
* @param primask The previous priority mask (PRIMASK) register value as
* returned by sl_hw_lock().
*/
static inline void
sl_hw_unlock(uint32_t primask)
{
if (primask == 0) {
__enable_irq();
}
}


When this came up a while ago on the forum there was an argument that it wasn't a problem. The well composed thread is:
http://forum.pjrc.com/threads/24098-I-don-t-think-critical-sections-work-(interrupt-locking)
Well, I just wrote my code the way I knew I needed it.

shanevanj
09-16-2014, 03:33 PM
I downloaded v1.20rc1 - so perhaps I need rc2 - anyone got a link for me - above is only rc1 as far I can see?

shanevanj
09-16-2014, 03:36 PM
Found rc2...installing ...http://www.pjrc.com/teensy/td_120-rc2/teensyduino.dmg

shanevanj
09-16-2014, 03:42 PM
Installed 1.20 rc2 serialevent() still not working - is there anything else to perhaps preconfigure ?

KurtE
09-16-2014, 04:25 PM
Another question, when you say it works on an Arduino? Which one? If normal Atmega328 or the like, than you are using IO pins 0,1 for the serial device and not USB input?

If so pins 0, 1 on the Teensy 3/3.1 as well as Arduino like Leonardo that are atmega 32u4 based is not the Serial object, but instead Serial1 object... So in that case you need to change your code to use serialEvent1() and Serial1 ...

PaulStoffregen
09-16-2014, 07:04 PM
Installed 1.20 rc2 serialevent() still not working - is there anything else to perhaps preconfigure ?

Please post this in this problem in the Suggestinos & Bug Reports section, as a new thread.

http://forum.pjrc.com/forums/4-Suggestions-amp-Bug-Reports

Remember the forum rule: Always post complete source code & details to reproduce any issue!

shanevanj
09-17-2014, 07:04 AM
Tested with an UNO (code below) using default USB connection as serial port inn both cases (teensy 3.1 and UNO) - UNO it works - teensy does not - moving this to Bug reports as suggested by Paul below.



char tempBuffer[100] = {0};
const int MaxInput = 10;
long prevMillis = 0;
int loopDelay = 1000;

// ---------------------------------------------------------------------------
void setup()
// ---------------------------------------------------------------------------
{
Serial.begin(115200);
while (!Serial); // wait for connection to terminal emulator
Serial.println("Enter something");
}

// ---------------------------------------------------------------------------
void loop()
// ---------------------------------------------------------------------------
{
if (millis() > prevMillis + loopDelay)
{
prevMillis = millis(); // reset count
Serial.println(".");
}
}

// ---------------------------------------------------------------------------
void serialEvent()
// ---------------------------------------------------------------------------
{
static unsigned int input_pos = 0;
//Serial.println("+"); // debug so we know this has been called
while (Serial.available())
{
char inByte = (char)Serial.read();
switch (inByte)
{
case '\n': // end of text
tempBuffer [input_pos] = 0; // terminating null byte
Serial.println(tempBuffer);
input_pos = 0;
break;

default:
// only allow alphabet and punctuation chrs
if (inByte >= 32 && inByte <= 126)
{
// keep adding if not full ... allow for terminating null byte
if (input_pos < (MaxInput - 1)) tempBuffer[input_pos++] = inByte;
} // end if((inByte ....
break;
}
}
}