I D/L’ed the ZIP file for V1.7 and here’s the code (I think) that runs when the 'copter is flying. Due to forum constraints, I have to split it into 2 halves to post in a code window.
/*
MultiWiiCopter by Alexandre Dubus
www.multiwii.com
April 2011 V1.7
This program is free software: you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation, either version 3 of the License, or
any later version. see <http://www.gnu.org/licenses/>
*/
/*******************************/
/****CONFIGURABLE PARAMETERS****/
/*******************************/
/* Set the minimum throttle command sent to the ESC (Electronic Speed Controller)
This is the minimum value that allow motors to run at a idle speed */
//#define MINTHROTTLE 1300 // for Turnigy Plush ESCs 10A
#define MINTHROTTLE 1120 // for Super Simple ESCs 10A
//#define MINTHROTTLE 1190
/* The type of multicopter */
//#define GIMBAL
//#define BI
#define TRI
//#define QUADP
//#define QUADX
//#define Y4
//#define Y6
//#define HEX6
//#define HEX6X
//#define FLYING_WING //experimental
#define YAW_DIRECTION 1 // if you want to reverse the yaw correction direction
//#define YAW_DIRECTION -1
#define I2C_SPEED 100000L //100kHz normal mode, this value must be used for a genuine WMP
//#define I2C_SPEED 400000L //400kHz fast mode, it works only with some WMP clones
#define PROMINI //Arduino type
//#define MEGA
//enable internal I2C pull ups
#define INTERNAL_I2C_PULLUPS
//****** advanced users settings *************
/* Failsave settings - added by MIS
Failsafe check pulse on THROTTLE channel. If the pulse is OFF (on only THROTTLE or on all channels) the failsafe procedure is initiated.
After FAILSAVE_DELAY time of pulse absence, the level mode is on (if ACC or nunchuk is avaliable), PITCH, ROLL and YAW is centered
and THROTTLE is set to FAILSAVE_THR0TTLE value. You must set this value to descending about 1m/s or so for best results.
This value is depended from your configuration, AUW and some other params.
Next, afrer FAILSAVE_OFF_DELAY the copter is disarmed, and motors is stopped.
If RC pulse coming back before reached FAILSAVE_OFF_DELAY time, after the small quard time the RC control is returned to normal.
If you use serial sum PPM, the sum converter must completly turn off the PPM SUM pusles for this FailSafe functionality.*/
#define FAILSAFE // Alex: comment this line if you want to deactivate the failsafe function
#define FAILSAVE_DELAY 10 // Guard time for failsafe activation after signal lost. 1 step = 0.1sec - 1sec in example
#define FAILSAVE_OFF_DELAY 200 // Time for Landing before motors stop in 0.1sec. 1 step = 0.1sec - 20sec in example
#define FAILSAVE_THR0TTLE (MINTHROTTLE + 200) // Throttle level used for landing - may be relative to MINTHROTTLE - as in this case
/* The following lines apply only for a pitch/roll tilt stabilization system
It is not compatible with Y6 or HEX6 or HEX6X
Uncomment the first line to activate it */
//#define SERVO_TILT
#define TILT_PITCH_MIN 1020 //servo travel min, don't set it below 1020
#define TILT_PITCH_MAX 2000 //servo travel max, max value=2000
#define TILT_PITCH_MIDDLE 1500 //servo neutral value
#define TILT_PITCH_PROP 10 //servo proportional (tied to angle) ; can be negative to invert movement
#define TILT_ROLL_MIN 1020
#define TILT_ROLL_MAX 2000
#define TILT_ROLL_MIDDLE 1500
#define TILT_ROLL_PROP 10
/* I2C gyroscope */
//#define ITG3200
//#define L3G4200D
/* I2C accelerometer */
//#define ADXL345
//#define BMA020
//#define BMA180
//#define NUNCHACK // if you want to use the nunckuk as a standalone I2C ACC without WMP
/* I2C barometer */
//#define BMP085
/* I2C magnetometer */
//#define HMC5843
//#define HMC5883
/* ADC accelerometer */ // for 5DOF from sparkfun, uses analog PIN A1/A2/A3
//#define ADCACC
/* The following lines apply only for specific receiver with only one PPM sum signal, on digital PIN 2
IF YOUR RECEIVER IS NOT CONCERNED, DON'T UNCOMMENT ANYTHING. Note this is mandatory for a Y6 setup
Select the right line depending on your radio brand. Feel free to modify the order in your PPM order is different */
//#define SERIAL_SUM_PPM PITCH,YAW,THROTTLE,ROLL,AUX1,AUX2,CAMPITCH,CAMROLL //For Graupner/Spektrum
//#define SERIAL_SUM_PPM ROLL,PITCH,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL //For Robe/Hitec/Futaba
//#define SERIAL_SUM_PPM PITCH,ROLL,THROTTLE,YAW,AUX1,AUX2,CAMPITCH,CAMROLL //For some Hitec/Sanwa/Others
/* interleaving delay in micro seconds between 2 readings WMP/NK in a WMP+NK config
if the ACC calibration time is very long (20 or 30s), try to increase this delay up to 4000
it is relevent only for a conf with NK */
#define INTERLEAVING_DELAY 3000
/* for V BAT monitoring
after the resistor divisor we should get [0V;5V]->[0;1023] on analog V_BATPIN
with R1=33k and R2=51k
vbat = [0;1023]*16/VBATSCALE */
#define VBAT // comment this line to suppress the vbat code
#define VBATSCALE 131 // change this value if readed Battery voltage is different than real voltage
#define VBATLEVEL1_3S 107 // 10,7V
#define VBATLEVEL2_3S 103 // 10,3V
#define VBATLEVEL3_3S 99 // 9.9V
/* when there is an error on I2C bus, we neutralize the values during a short time. expressed in microseconds
it is relevent only for a conf with at least a WMP */
#define NEUTRALIZE_DELAY 100000
/* this is the value for the ESCs when thay are not armed
in some cases, this value must be lowered down to 900 for some specific ESCs */
#define MINCOMMAND 1000
/* this is the maximum value for the ESCs at full power
this value can be increased up to 2000 */
#define MAXTHROTTLE 1850
/* This is the speed of the serial interface. 115200 kbit/s is the best option for a USB connection.*/
#define SERIAL_COM_SPEED 115200
/* In order to save space, it's possibile to desactivate the LCD configuration functions
comment this line only if you don't plan to used a LCD */
#define LCD_CONF
/* to use Cat's whisker TEXTSTAR LCD, uncomment following line.
Pleae note this display needs a full 4 wire connection to (+5V, Gnd, RXD, TXD )
Configure display as follows: 115K baud, and TTL levels for RXD and TXD, terminal mode
NO rx / tx line reconfiguration, use natural pins */
//#define LCD_TEXTSTAR
/* motors will not spin when the throttle command is in low position
this is an alternative method to stop immediately the motors */
//#define MOTOR_STOP
/* some radios have not a neutral point centered on 1500. can be changed here */
#define MIDRC 1500
/* experimental
camera trigger function : activated via AUX1 UP, servo output=A2 */
//#define CAMTRIG
#define CAM_SERVO_HIGH 2000 // the position of HIGH state servo
#define CAM_SERVO_LOW 1020 // the position of LOW state servo
#define CAM_TIME_HIGH 1000 // the duration of HIGH state servo expressed in ms
#define CAM_TIME_LOW 1000 // the duration of LOW state servo expressed in ms
/* you can change the tricopter servo travel here */
#define TRI_YAW_CONSTRAINT_MIN 1020
#define TRI_YAW_CONSTRAINT_MAX 2000
#define TRI_YAW_MIDDLE 1500
//****** end of advanced users settings *************
/**************************************/
/****END OF CONFIGURABLE PARAMETERS****/
/**************************************/
#include <EEPROM.h>
#define LEDPIN 13 // will be changed for MEGA in a future version
#define POWERPIN 12 // will be changed for MEGA in a future version
#define V_BATPIN 3 // Analog PIN 3
#define STABLEPIN // will be defined for MEGA in a future version
#if defined(PROMINI)
#define LEDPIN_PINMODE pinMode (13, OUTPUT);
#define LEDPIN_SWITCH PINB |= 1<<5; //switch LEDPIN state (digital PIN 13)
#define LEDPIN_OFF PORTB &= ~(1<<5);
#define LEDPIN_ON PORTB |= (1<<5);
#define BUZZERPIN_PINMODE pinMode (8, OUTPUT);
#define BUZZERPIN_ON PORTB |= 1;
#define BUZZERPIN_OFF PORTB &= ~1;
#define POWERPIN_PINMODE pinMode (12, OUTPUT);
#define POWERPIN_ON PORTB |= 1<<4;
#define POWERPIN_OFF PORTB &= ~(1<<4); //switch OFF WMP, digital PIN 12
#define I2C_PULLUPS_ENABLE PORTC |= 1<<4; PORTC |= 1<<5; // PIN A4&A5 (SDA&SCL)
#define I2C_PULLUPS_DISABLE PORTC &= ~(1<<4); PORTC &= ~(1<<5);
#define PINMODE_LCD pinMode(0, OUTPUT);
#define LCDPIN_OFF PORTD &= ~1;
#define LCDPIN_ON PORTD |= 1;
#define DIGITAL_SERVO_TRI_PINMODE pinMode(3,OUTPUT); //also right servo for BI COPTER
#define DIGITAL_SERVO_TRI_HIGH PORTD |= 1<<3;
#define DIGITAL_SERVO_TRI_LOW PORTD &= ~(1<<3);
#define DIGITAL_TILT_PITCH_PINMODE pinMode(A0,OUTPUT);
#define DIGITAL_TILT_PITCH_HIGH PORTC |= 1<<0;
#define DIGITAL_TILT_PITCH_LOW PORTC &= ~(1<<0);
#define DIGITAL_TILT_ROLL_PINMODE pinMode(A1,OUTPUT);
#define DIGITAL_TILT_ROLL_HIGH PORTC |= 1<<1;
#define DIGITAL_TILT_ROLL_LOW PORTC &= ~(1<<1);
#define DIGITAL_BI_LEFT_PINMODE pinMode(11,OUTPUT);
#define DIGITAL_BI_LEFT_HIGH PORTB |= 1<<3;
#define DIGITAL_BI_LEFT_LOW PORTB &= ~(1<<3);
#define PPM_PIN_INTERRUPT attachInterrupt(0, rxInt, RISING); //PIN 0
#define MOTOR_ORDER 9,10,11,3,6,5 //for a quad+: rear,right,left,front
#define DIGITAL_CAM_PINMODE pinMode(A2,OUTPUT);
#define DIGITAL_CAM_HIGH PORTC |= 1<<2;
#define DIGITAL_CAM_LOW PORTC &= ~(1<<2);
//RX PIN assignment inside the port //for PORTD
#define THROTTLEPIN 2
#define ROLLPIN 4
#define PITCHPIN 5
#define YAWPIN 6
#define AUX1PIN 7
#define AUX2PIN 7 //unused just for compatibility with MEGA
#define CAM1PIN 7 //unused just for compatibility with MEGA
#define CAM2PIN 7 //unused just for compatibility with MEGA
#define ISR_UART ISR(USART_UDRE_vect)
#endif
#if defined(MEGA)
#define LEDPIN_PINMODE pinMode (13, OUTPUT);
#define LEDPIN_SWITCH PINB |= (1<<7);
#define LEDPIN_OFF PORTB &= ~(1<<7);
#define LEDPIN_ON PORTB |= (1<<7);
#define BUZZERPIN_PINMODE pinMode (31, OUTPUT);
#define BUZZERPIN_ON PORTC |= 1<<6;
#define BUZZERPIN_OFF PORTC &= ~1<<6;
#define POWERPIN_PINMODE pinMode (12, OUTPUT);
#define POWERPIN_ON PORTB |= 1<<6;
#define POWERPIN_OFF PORTB &= ~(1<<6);
#define I2C_PULLUPS_ENABLE PORTD |= 1<<0; PORTD |= 1<<1; // PIN 20&21 (SDA&SCL)
#define I2C_PULLUPS_DISABLE PORTD &= ~(1<<0); PORTD &= ~(1<<1);
#define PINMODE_LCD pinMode(0, OUTPUT);
#define LCDPIN_OFF PORTE &= ~1; //switch OFF digital PIN 0
#define LCDPIN_ON PORTE |= 1; //switch OFF digital PIN 0
#define DIGITAL_SERVO_TRI_PINMODE pinMode(2,OUTPUT); //PIN 2 //also right servo for BI COPTER
#define DIGITAL_SERVO_TRI_HIGH PORTE |= 1<<4;
#define DIGITAL_SERVO_TRI_LOW PORTE &= ~(1<<4);
#define DIGITAL_TILT_PITCH_PINMODE pinMode(A0,OUTPUT); // not the final choice
#define DIGITAL_TILT_PITCH_HIGH PORTF |= 1<<0;
#define DIGITAL_TILT_PITCH_LOW PORTF &= ~(1<<0);
#define DIGITAL_TILT_ROLL_PINMODE pinMode(A1,OUTPUT); // not the final choice
#define DIGITAL_TILT_ROLL_HIGH PORTF |= 1<<1;
#define DIGITAL_TILT_ROLL_LOW PORTF &= ~(1<<1);
#define DIGITAL_BI_LEFT_PINMODE pinMode(6,OUTPUT);
#define DIGITAL_BI_LEFT_HIGH PORTE |= 1<<6;
#define DIGITAL_BI_LEFT_LOW PORTE &= ~(1<<6);
#define PPM_PIN_INTERRUPT attachInterrupt(4, rxInt, RISING); //PIN 19
#define MOTOR_ORDER 3,5,6,2,7,8 //for a quad+: rear,right,left,front //+ for y6: 7:under right 8:under left
#define DIGITAL_CAM_PINMODE pinMode(A2,OUTPUT); // not the final choice
#define DIGITAL_CAM_HIGH PORTF |= 1<<2;
#define DIGITAL_CAM_LOW PORTF &= ~(1<<2);
//RX PIN assignment inside the port //for PORTK
#define THROTTLEPIN 0 //PIN 62 = PIN A8
#define ROLLPIN 1 //PIN 63 = PIN A9
#define PITCHPIN 2 //PIN 64 = PIN A10
#define YAWPIN 3 //PIN 65 = PIN A11
#define AUX1PIN 4 //PIN 66 = PIN A12
#define AUX2PIN 5 //PIN 67 = PIN A13
#define CAM1PIN 6 //PIN 68 = PIN A14
#define CAM2PIN 7 //PIN 69 = PIN A15
#define ISR_UART ISR(USART0_UDRE_vect)
#endif
#if defined(BI) || defined(TRI) || defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING) || defined(CAMTRIG)
#define SERVO
#endif
#if defined(ADXL345) || defined(BMA020) || defined(BMA180) || defined(NUNCHACK)
#define I2C_ACC
#endif
#if defined(GIMBAL) || defined(FLYING_WING)
#define NUMBER_MOTOR 0
#elif defined(BI)
#define NUMBER_MOTOR 2
#elif defined(TRI)
#define NUMBER_MOTOR 3
#elif defined(QUADP) || defined(QUADX) || defined(Y4)
#define NUMBER_MOTOR 4
#elif defined(Y6) || defined(HEX6) || defined(HEX6X)
#define NUMBER_MOTOR 6
#endif
/*********** RC alias *****************/
#define ROLL 0
#define PITCH 1
#define YAW 2
#define THROTTLE 3
#define AUX1 4
#define AUX2 5
#define CAMPITCH 6
#define CAMROLL 7
static uint32_t previousTime;
static uint32_t neutralizeTime;
static uint32_t rcTime;
static uint32_t currentTime;
static uint16_t cycleTime; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
static uint16_t meanTime = 2000; // this is the average time of the loop: around 2ms for a WMP config and 6ms for a NK+WMP config
static uint16_t calibratingA; // the calibration is done is the main loop. Calibrating decreases at each cycle down to 0, then we enter in a normal mode.
static uint16_t calibratingG;
static uint8_t armed = 0;
static int16_t acc_1G = 200; //this is the 1G measured acceleration (nunchuk)
static int16_t acc_25deg = 85; //this is the the ACC value measured on x or y axis for a 25deg inclination (nunchuk) = acc_1G * sin(25)
static uint8_t nunchukPresent = 0;
static uint8_t accPresent = 0; //I2C or ADC acc present
static uint8_t gyroPresent = 0; //I2C or ADC gyro present (other than WMP)
static uint8_t magPresent = 0; //I2C or ADC compass present
static uint8_t baroPresent = 0; //I2C or ADC baro present
static uint8_t accMode = 0; //if level mode is a activated
static uint8_t magMode = 0; //if compass heading hold is a activated
static uint8_t baroMode = 0; //if altitude hold is activated
static int16_t accADC[3];
static int16_t gyroADC[3];
static int16_t magADC[3];
static int16_t heading, magHold;
static int16_t altitudeSmooth = 0;
static uint8_t calibratedACC = 0;
static uint8_t vbat; //battery voltage in 0.1V steps
// *********************
// I2C general functions
// *********************
// Mask prescaler bits : only 5 bits of TWSR defines the status of each I2C request
#define TW_STATUS_MASK (1<<TWS7) | (1<<TWS6) | (1<<TWS5) | (1<<TWS4) | (1<<TWS3)
#define TW_STATUS (TWSR & TW_STATUS_MASK)
void i2c_init(void) {
#if defined(INTERNAL_I2C_PULLUPS)
I2C_PULLUPS_ENABLE
#else
I2C_PULLUPS_DISABLE
#endif
TWSR = 0; // no prescaler => prescaler = 1
TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
TWCR = 1 << TWEN; // enable twi module, no interrupt
}
void i2c_rep_start(uint8_t address) {
TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN) | (1 << TWSTO); // send REAPEAT START condition
waitTransmissionI2C(); // wait until transmission completed
checkStatusI2C(); // check value of TWI Status Register
TWDR = address; // send device address
TWCR = (1 << TWINT) | (1 << TWEN);
waitTransmissionI2C(); // wail until transmission completed
checkStatusI2C(); // check value of TWI Status Register
}
void i2c_write(uint8_t data ) {
TWDR = data; // send data to the previously addressed device
TWCR = (1 << TWINT) | (1 << TWEN);
waitTransmissionI2C(); // wait until transmission completed
checkStatusI2C(); // check value of TWI Status Register
}
uint8_t i2c_readAck() {
TWCR = (1 << TWINT) | (1 << TWEN) | (1 << TWEA);
waitTransmissionI2C();
return TWDR;
}
uint8_t i2c_readNak(void) {
TWCR = (1 << TWINT) | (1 << TWEN);
waitTransmissionI2C();
return TWDR;
}
void waitTransmissionI2C() {
uint8_t count = 255;
while (count-- > 0 && !(TWCR & (1 << TWINT)) );
if (count < 2) { //we are in a blocking state => we don't insist
TWCR = 0; //and we force a reset on TWINT register
neutralizeTime = micros(); //we take a timestamp here to neutralize the value during a short delay after the hard reset
}
}
void checkStatusI2C() {
if ( TW_STATUS == 0xF8) { //TW_NO_INFO : this I2C error status indicates a wrong I2C communication.
// WMP does not respond anymore => we do a hard reset. I did not find another way to solve it. It takes only 13ms to reset and init to WMP or WMP+NK
TWCR = 0;
POWERPIN_OFF //switch OFF WMP
delay(1);
POWERPIN_ON //switch ON WMP
delay(10);
#if defined(ITG3200) || defined(L3G4200D)
#else
i2c_WMP_init(0);
#endif
neutralizeTime = micros(); //we take a timestamp here to neutralize the WMP or WMP+NK values during a short delay after the hard reset
}
}
// **************************
// I2C Barometer BOSCH BMP085
// **************************
// I2C adress: 0xEE (8bit) 0x77 (7bit)
// principle:
// 1) read the calibration register (only once at the initialization)
// 2) read uncompensated temperature (not mandatory at every cycle)
// 3) read uncompensated pressure
// 4) raw temp + raw pressure => calculation of the adjusted pressure
// the following code uses the maximum precision setting (oversampling setting 3)
// sensor registers from the BOSCH BMP085 datasheet
#if defined(BMP085)
static int16_t ac1, ac2, ac3, b1, b2, mb, mc, md;
static uint16_t ac4, ac5, ac6;
static uint16_t ut; //uncompensated T
static uint32_t up; //uncompensated P
int32_t temperature = 0;
int32_t pressure = 0;
int16_t altitude = 0;
static int16_t altitudeZero;
static int16_t altitudeHold;
void i2c_Baro_init() {
delay(10);
ac1 = i2c_BMP085_readIntRegister(0xAA);
ac2 = i2c_BMP085_readIntRegister(0xAC);
ac3 = i2c_BMP085_readIntRegister(0xAE);
ac4 = i2c_BMP085_readIntRegister(0xB0);
ac5 = i2c_BMP085_readIntRegister(0xB2);
ac6 = i2c_BMP085_readIntRegister(0xB4);
b1 = i2c_BMP085_readIntRegister(0xB6);
b2 = i2c_BMP085_readIntRegister(0xB8);
mb = i2c_BMP085_readIntRegister(0xBA);
mc = i2c_BMP085_readIntRegister(0xBC);
md = i2c_BMP085_readIntRegister(0xBE);
baroPresent = 1;
i2c_BMP085_calibrate();
}
// read a 16 bit register
int16_t i2c_BMP085_readIntRegister(unsigned char r) {
uint8_t msb, lsb;
i2c_rep_start(0xEE + 0);
i2c_write(r);
i2c_rep_start(0xEE + 1);//I2C read direction => 1
msb = i2c_readAck();
lsb = i2c_readNak();
return (((int16_t)msb << 8) | ((int16_t)lsb));
}
// read uncompensated temperature value: send command first
void i2c_BMP085_readUT_Command() {
i2c_rep_start(0xEE + 0);
i2c_write(0xf4);
i2c_write(0x2e);
i2c_rep_start(0xEE + 0);
i2c_write(0xF6);
}
// read uncompensated temperature value: read result bytes
// the datasheet suggests a delay of 4.5 ms after the send command
uint16_t i2c_BMP085_readUT_Result() {
uint8_t msb, lsb;
i2c_rep_start(0xEE + 1);//I2C read direction => 1
msb = i2c_readAck();
lsb = i2c_readNak();
return (((uint16_t)msb << 8) | ((uint16_t)lsb));
}
// read uncompensated pressure value: send command first
void i2c_BMP085_readUP_Command () {
i2c_rep_start(0xEE + 0);
i2c_write(0xf4);
i2c_write(0xf4); //control register value for oversampling setting 3
i2c_rep_start(0xEE + 0); //I2C write direction => 0
i2c_write(0xf6);
}
// read uncompensated pressure value: read result bytes
// the datasheet suggests a delay of 25.5 ms (oversampling settings 3) after the send command
uint32_t i2c_BMP085_readUP_Result () {
uint8_t msb, lsb, xlsb;
i2c_rep_start(0xEE + 1);//I2C read direction => 1
msb = i2c_readAck();
lsb = i2c_readAck();
xlsb = i2c_readNak();
return (((uint32_t)msb << 16) | ((uint32_t)lsb << 8) | ((uint32_t)xlsb)) >> 5;
}
// deduction of true temperature and pressure from sensor, code is described in the BMP085 specs
void i2c_BMP085_CompensatedSensor() {
int32_t x1, x2, x3, b3, b5, b6, p;
uint32_t b4, b7;
uint16_t a, b;
//calculate true temperature
x1 = (int32_t)(ut - ac6) * ac5 >> 15;
x2 = ((int32_t) mc << 11) / (x1 + md);
b5 = x1 + x2;
temperature = (b5 + 8) >> 4;
//calculate true pressure
b6 = b5 - 4000;
x1 = (b2 * (b6 * b6 >> 12)) >> 11;
x2 = ac2 * b6 >> 11;
x3 = x1 + x2;
b3 = ( ( ((int32_t) ac1 * 4 + x3) << 3 ) + 2) >> 2;
x1 = ac3 * b6 >> 13;
x2 = (b1 * (b6 * b6 >> 12)) >> 16;
x3 = ((x1 + x2) + 2) >> 2;
b4 = (ac4 * (uint32_t) (x3 + 32768)) >> 15;
b7 = (up - b3) * (50000 >> 3);
p = b7 < 0x80000000 ? (b7 * 2) / b4 : (b7 / b4) * 2;
x1 = (p >> 8) * (p >> 8);
x1 = (x1 * 3038) >> 16;
x2 = (-7357 * p) >> 16;
pressure = p + ((x1 + x2 + 3791) >> 4);
}
//in a whole cycle: we read temperature one time and pressure 5 times
void i2c_Baro_update() {
static uint32_t t;
static uint8_t state1 = 0, state2 = 0;
if ( (micros() - t ) < 30000 ) return; //each read is spaced by 30ms
t = micros();
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, BMP085 is ok with this speed
if (state1 == 0) {
if (state2 == 0) {
i2c_BMP085_readUT_Command();
state2 = 1;
} else {
ut = i2c_BMP085_readUT_Result();
state2 = 0;
state1 = 5;
}
} else {
if (state2 == 0) {
i2c_BMP085_readUP_Command();
state2 = 1;
} else {
up = i2c_BMP085_readUP_Result();
state2 = 0;
state1--;
i2c_BMP085_CompensatedSensor();
altitude = (1.0 - pow(float(pressure) / 101325.0, 0.190295)) * 443300 - altitudeZero; // altitude in decimeter from starting point
if ( abs(altitude - altitudeSmooth) < 100 ) //avoid altitude spike
altitudeSmooth = (altitudeSmooth * 7 + altitude + 4) / 8;
}
}
}
void i2c_BMP085_calibrate() {
altitudeZero = 0;
for (uint8_t i = 0; i < 7; i++) {
i2c_Baro_update();
delay(35);
}
altitudeZero = altitude;
}
#endif
// **************************
// I2C Accelerometer ADXL345
// **************************
// I2C adress: 0x3A (8bit) 0x1D (7bit)
// principle:
// 1) CS PIN must be linked to VCC to select the I2C mode
// 2) SD0 PIN must be linked to VCC to select the right I2C adress
// 3) bit b00000100 must be set on register 0x2D to read data (only once at the initialization)
// 4) bits b00001011 must be set on register 0x31 to select the data format (only once at the initialization)
#if defined(ADXL345)
static uint8_t rawADC_ADXL345[6];
void i2c_ACC_init () {
delay(10);
i2c_rep_start(0x3A + 0); // I2C write direction
i2c_write(0x2D); // register 2D Power CTRL
i2c_write(1 << 3); // Set measure bit 3 on
i2c_rep_start(0x3A + 0); // I2C write direction
i2c_write(0x31); // DATA_FORMAT register
i2c_write(0x0B); // Set bits 3(full range) and 1 0 on (+/- 16g-range)
i2c_rep_start(0x3A + 0); // I2C write direction
i2c_write(0x2C); // BW_RATE
i2c_write(8 + 2 + 1); // 200Hz sampling (see table 5 of the spec)
acc_1G = 250;
acc_25deg = 106; // = acc_1G * sin(25 deg)
accPresent = 1;
}
void i2c_ACC_getADC () {
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, ADXL435 is ok with this speed
i2c_rep_start(0x3A); // I2C write direction
i2c_write(0x32); // Start multiple read at reg 0x32 ADX
i2c_rep_start(0x3A + 1); // I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_ADXL345[i] = i2c_readAck();
}
rawADC_ADXL345[5] = i2c_readNak();
accADC[ROLL] = - ((rawADC_ADXL345[3] << 8) | rawADC_ADXL345[2]);
accADC[PITCH] = ((rawADC_ADXL345[1] << 8) | rawADC_ADXL345[0]);
accADC[YAW] = - ((rawADC_ADXL345[5] << 8) | rawADC_ADXL345[4]);
}
#endif
// **************************
// contribution from opie11 (rc-grooups)
// I2C Accelerometer BMA180
// **************************
// I2C adress: 0x80 (8bit) 0x40 (7bit)
#if defined(BMA180)
static uint8_t rawADC_BMA180[6];
void i2c_ACC_init () {
delay(10);
i2c_rep_start(0x80 + 0); // I2C write direction
i2c_write(0x0D); // ctrl_reg0
i2c_write(1 << 4); // Set bit 4 to 1 to enable writing
i2c_rep_start(0x80 + 0);
i2c_write(0x35); //
i2c_write(3 << 1); // range set to 3. 2730 1G raw data. With /10 divisor on acc_ADC, more in line with other sensors and works with the GUI
i2c_rep_start(0x80 + 0);
i2c_write(0x20); // bw_tcs reg: bits 4-7 to set bw
i2c_write(0 << 4); // bw to 10Hz (low pass filter)
acc_1G = 273;
acc_25deg = 113; // = acc_1G * sin(25 deg)
accPresent = 1;
}
void i2c_ACC_getADC () {
TWBR = ((16000000L / 400000L) - 16) / 2; // Optional line. Sensor is good for it in the spec.
i2c_rep_start(0x80); // I2C write direction
i2c_write(0x02); // Start multiple read at reg 0x02 acc_x_lsb
i2c_rep_start(0x80 + 1); // I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_BMA180[i] = i2c_readAck();
}
rawADC_BMA180[5] = i2c_readNak();
accADC[ROLL] = - (((rawADC_BMA180[1] << 8) | (rawADC_BMA180[0])) >> 2) / 10; // opie settings: + ; FFIMU: -
accADC[PITCH] = - (((rawADC_BMA180[3] << 8) | (rawADC_BMA180[2])) >> 2) / 10;
accADC[YAW] = - (((rawADC_BMA180[5] << 8) | (rawADC_BMA180[4])) >> 2) / 10;
}
#endif
// **************
// contribution from Point65 and mgros (rc-grooups)
// BMA020 I2C
// **************
// I2C adress: 0x70 (8bit)
#if defined(BMA020)
static uint8_t rawADC_BMA020[6];
void i2c_ACC_init() {
byte control;
i2c_rep_start(0x70); // I2C write direction
i2c_write(0x15); //
i2c_write(0x80); // Write B10000000 at 0x15 init BMA020
i2c_rep_start(0x70); //
i2c_write(0x14); //
i2c_write(0x71); //
i2c_rep_start(0x71); //
control = i2c_readNak();
control = control >> 5; //ensure the value of three fist bits of reg 0x14 see BMA020 documentation page 9
control = control << 2;
control = control | 0x00; //Range 2G 00
control = control << 3;
control = control | 0x00; //Bandwidth 25 Hz 000
i2c_rep_start(0x70); // I2C write direction
i2c_write(0x14); // Start multiple read at reg 0x32 ADX
i2c_write(control);
acc_1G = 240;
acc_25deg = 101; // = acc_1G * sin(25 deg)
accPresent = 1;
}
void i2c_ACC_getADC() {
TWBR = ((16000000L / 400000L) - 16) / 2;
i2c_rep_start(0x70); // I2C write direction
i2c_write(0x02); // Start multiple read at reg 0x32 ADX
i2c_write(0x71);
i2c_rep_start(0x71); //I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_BMA020[i] = i2c_readAck();
}
rawADC_BMA020[5] = i2c_readNak();
accADC[ROLL] = (((rawADC_BMA020[1]) << 8) | ((rawADC_BMA020[0] >> 1) << 1)) / 64;
accADC[PITCH] = (((rawADC_BMA020[3]) << 8) | ((rawADC_BMA020[2] >> 1) << 1)) / 64;
accADC[YAW] = -(((rawADC_BMA020[5]) << 8) | ((rawADC_BMA020[4] >> 1) << 1)) / 64;
}
#endif
// **************************
// contribution from Ciskje
// I2C Gyroscope L3G4200D
// **************************
#if defined(L3G4200D)
static uint8_t rawADC_L3G4200D[6];
void i2c_Gyro_init() {
delay(100);
i2c_rep_start(0XD2 + 0); // CTRL_REG1
i2c_write(0x20); // 400Hz ODR, 20hz filter, run!
i2c_write(0x8F);
i2c_rep_start(0XD2 + 0); // CTRL_REG5
i2c_write(0x24); // low pass filter enable
i2c_write(0x02);
gyroPresent = 1;
}
void i2c_Gyro_getADC () {
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
i2c_rep_start(0XD2); // I2C write direction
i2c_write(0x80 | 0x28); // Start multiple read
i2c_rep_start(0XD2 + 1); // I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_L3G4200D[i] = i2c_readAck();
}
rawADC_L3G4200D[5] = i2c_readNak();
gyroADC[ROLL] = ((rawADC_L3G4200D[1] << 8) | rawADC_L3G4200D[0]) / 20 ;
gyroADC[PITCH] = ((rawADC_L3G4200D[3] << 8) | rawADC_L3G4200D[2]) / 20 ;
gyroADC[YAW] = -((rawADC_L3G4200D[5] << 8) | rawADC_L3G4200D[4]) / 20 ;
}
#endif
// **************************
// I2C Gyroscope ITG3200
// **************************
// I2C adress: 0xD2 (8bit) 0x69 (7bit) // for sparkfun breakout board default jumper
// I2C adress: 0xD0 (8bit) 0x68 (7bit) // for FreeFlight IMU board default jumper
// principle:
// 1) VIO is connected to VDD
// 2) I2C adress is set to 0x69 (AD0 PIN connected to VDD)
// or 2) I2C adress is set to 0x68 (AD0 PIN connected to GND) <- this is the case for the code here
// 3) sample rate = 1000Hz ( 1kHz/(div+1) )
#if defined(ITG3200)
static uint8_t rawADC_ITG3200[6];
void i2c_Gyro_init() {
delay(100);
i2c_rep_start(0XD0 + 0); // I2C write direction
i2c_write(0x3E); // Power Management register
i2c_write(0x80); // reset device
i2c_write(0x16); // register DLPF_CFG - low pass filter configuration & sample rate
i2c_write(0x1D); // 10Hz Low Pass Filter Bandwidth - Internal Sample Rate 1kHz
i2c_write(0x3E); // Power Management register
i2c_write(0x01); // PLL with X Gyro reference
delay(100);
gyroPresent = 1;
}
void i2c_Gyro_getADC () {
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
i2c_rep_start(0XD0); // I2C write direction
i2c_write(0X1D); // Start multiple read
i2c_rep_start(0XD0 + 1); // I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_ITG3200[i] = i2c_readAck();
}
rawADC_ITG3200[5] = i2c_readNak();
gyroADC[ROLL] = + ( ((rawADC_ITG3200[2] << 8) | rawADC_ITG3200[3]) / 5 / 4 );
gyroADC[PITCH] = - ( ((rawADC_ITG3200[0] << 8) | rawADC_ITG3200[1]) / 5 / 4 );
gyroADC[YAW] = - ( ((rawADC_ITG3200[4] << 8) | rawADC_ITG3200[5]) / 5 / 4 );
}
#endif
// **************************
// I2C Compass HMC5843
// **************************
// I2C adress: 0x3C (8bit) 0x1E (7bit)
#if defined(HMC5843)
static uint8_t rawADC_HMC5843[6];
void i2c_Mag_init() {
delay(100);
i2c_rep_start(0X3C + 0); // I2C write direction
i2c_write(0x02); // Write to Mode register
i2c_write(0x00); // Continuous-Conversion Mode
magPresent = 1;
}
void i2c_Mag_getADC() {
static uint32_t t;
if ( (micros() - t ) < 100000 ) return; //each read is spaced by 100ms
t = micros();
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
i2c_rep_start(0X3C); // I2C write direction
i2c_write(0X03); // Start multiple read
i2c_rep_start(0X3C + 1); // I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_HMC5843[i] = i2c_readAck();
}
rawADC_HMC5843[5] = i2c_readNak();
magADC[ROLL] = ((rawADC_HMC5843[0] << 8) | rawADC_HMC5843[1]);
magADC[PITCH] = ((rawADC_HMC5843[2] << 8) | rawADC_HMC5843[3]);
magADC[YAW] = - ((rawADC_HMC5843[4] << 8) | rawADC_HMC5843[5]);
}
#endif
// **************************
// I2C Compass HMC5883
// **************************
// I2C adress: 0x3C (8bit) 0x1E (7bit)
#if defined(HMC5883)
static uint8_t rawADC_HMC5883[6];
void i2c_Mag_init() {
delay(100);
i2c_rep_start(0X3C + 0); // I2C write direction
i2c_write(0x02); // Write to Mode register
i2c_write(0x00); // Continuous-Conversion Mode
magPresent = 1;
}
void i2c_Mag_getADC () {
static uint32_t t;
if ( (micros() - t ) < 100000 ) return; //each read is spaced by 100ms
t = micros();
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz
i2c_rep_start(0X3C); // I2C write direction
i2c_write(0X03); // Start multiple read
i2c_rep_start(0X3C + 1); // I2C read direction => 1
for (uint8_t i = 0; i < 5; i++) {
rawADC_HMC5883[i] = i2c_readAck();
}
rawADC_HMC5883[5] = i2c_readNak();
magADC[ROLL] = ((rawADC_HMC5883[4] << 8) | rawADC_HMC5883[5]); // note: manual calibration: +300
magADC[PITCH] = -((rawADC_HMC5883[0] << 8) | rawADC_HMC5883[1]); // +200
magADC[YAW] = -((rawADC_HMC5883[2] << 8) | rawADC_HMC5883[3]); // +50
}
#endif
// **************************
// standalone I2C NUNCHUK
// **************************
#if defined(NUNCHACK)
static uint8_t rawADC_NUN[6];
void i2c_ACC_init() {
i2c_rep_start(0xA4 + 0);//I2C write direction => 0
i2c_write(0xF0);
i2c_write(0x55);
i2c_rep_start(0xA4 + 0);//I2C write direction => 0
i2c_write(0xFB);
i2c_write(0x00);
delay(250);
accPresent = 1;
}
void i2c_ACC_getADC() {
TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate. !! you must check if the nunchuk is ok with this freq
i2c_rep_start(0xA4 + 0);//I2C write direction => 0
i2c_write(0x00);
i2c_rep_start(0xA4 + 1);//I2C read direction => 1
for (uint8_t i = 0; i < 5; i++)
rawADC_NUN[i] = i2c_readAck();
rawADC_NUN[5] = i2c_readNak();
accADC[ROLL] = ( (rawADC_NUN[3] << 2) + ((rawADC_NUN[5] >> 4) & 0x2) );
accADC[PITCH] = - ( (rawADC_NUN[2] << 2) + ((rawADC_NUN[5] >> 3) & 0x2) );
accADC[YAW] = - ( ((rawADC_NUN[4] & 0xFE) << 2) + ((rawADC_NUN[5] >> 5) & 0x6) );
}
#endif
// **************************
// I2C Wii Motion Plus
// **************************
// I2C adress 1: 0xA6 (8bit) 0x53 (7bit)
// I2C adress 2: 0xA4 (8bit) 0x52 (7bit)
static uint8_t rawADC_WMP[6];
void i2c_WMP_init(uint8_t d) {
delay(d);
i2c_rep_start(0xA6 + 0);//I2C write direction => 0
i2c_write(0xF0);
i2c_write(0x55);
delay(d);
i2c_rep_start(0xA6 + 0);//I2C write direction => 0
i2c_write(0xFE);
i2c_write(0x05);
delay(d);
if (d > 0) {
uint8_t numberAccRead = 0;
for (uint8_t i = 0; i < 100; i++) {
delay(3);
if (rawIMU(0) == 0) numberAccRead++; // we detect here is nunchuk extension is available
}
if (numberAccRead > 25)
nunchukPresent = 1;
delay(10);
}
}
void i2c_WMP_getRawADC() {
TWBR = ((16000000L / I2C_SPEED) - 16) / 2; // change the I2C clock rate
i2c_rep_start(0xA4 + 0);//I2C write direction => 0
i2c_write(0x00);
i2c_rep_start(0xA4 + 1);//I2C read direction => 1
for (uint8_t i = 0; i < 5; i++)
rawADC_WMP[i] = i2c_readAck();
rawADC_WMP[5] = i2c_readNak();
}
// **************************
// ADC ACC
// **************************
#if defined(ADCACC)
void adc_ACC_init() {
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
}
void adc_ACC_getRawADC() {
accADC[ROLL] = -analogRead(A1);
accADC[PITCH] = -analogRead(A2);
accADC[YAW] = -analogRead(A3);
acc_1G = 75;
acc_25deg = 32; // = acc_1G * sin(25 deg)
accPresent = 1;
}
#endif
// **************
// gyro+acc IMU
// **************
static int16_t gyroData[3] = {0, 0, 0};
static int16_t gyroZero[3] = {0, 0, 0};
static int16_t accZero[3] = {0, 0, 0};
static int16_t angle[2]; //absolute angle inclination in multiple of 0.1 degree
static int16_t accSmooth[3]; //projection of smoothed and normalized gravitation force vector on x/y/z axis, as measured by accelerometer
uint8_t rawIMU(uint8_t withACC) { //if the WMP or NK are oriented differently, it can be changed here
if (withACC) {
#if defined(I2C_ACC)
i2c_ACC_getADC();
#endif
#if defined(ADCACC)
adc_ACC_getRawADC();
#endif
}
#if defined(ITG3200) || defined(L3G4200D)
i2c_Gyro_getADC();
return 1;
#else
i2c_WMP_getRawADC();
if ( (rawADC_WMP[5] & 0x02) == 0x02 && (rawADC_WMP[5] & 0x01) == 0 ) { // motion plus data
gyroADC[ROLL] = - ( ((rawADC_WMP[5] >> 2) << 8) + rawADC_WMP[2] );
gyroADC[PITCH] = - ( ((rawADC_WMP[4] >> 2) << 8) + rawADC_WMP[1] );
gyroADC[YAW] = - ( ((rawADC_WMP[3] >> 2) << 8) + rawADC_WMP[0] );
return 1;
} else if ( (rawADC_WMP[5] & 0x02) == 0 && (rawADC_WMP[5] & 0x01) == 0) { //nunchuk data
#if defined(I2C_ACC) || defined(ADCACC)
return 2;
#else
accADC[ROLL] = ( (rawADC_WMP[3] << 2) + ((rawADC_WMP[5] >> 4) & 0x2) );
accADC[PITCH] = - ( (rawADC_WMP[2] << 2) + ((rawADC_WMP[5] >> 3) & 0x2) );
accADC[YAW] = - ( ((rawADC_WMP[4] & 0xFE) << 2) + ((rawADC_WMP[5] >> 5) & 0x6) );
return 0;
#endif
} else
return 2;
#endif
}
uint8_t updateIMU(uint8_t withACC) {
static int32_t g[3];
static int32_t a[3];
uint8_t axis;
static int16_t previousGyroADC[3] = {0, 0, 0};
uint8_t r;
r = rawIMU(withACC);
if (currentTime < (neutralizeTime + NEUTRALIZE_DELAY)) {//we neutralize data in case of blocking+hard reset state
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] = 0;
accADC[axis] = 0;
}
accADC[YAW] = acc_1G;
} else {
if (r == 1) { //gyro
if (calibratingG > 0) {
for (axis = 0; axis < 3; axis++) {
if (calibratingG > 1) {
if (calibratingG == 400) g[axis] = 0;
g[axis] += gyroADC[axis];
gyroADC[axis] = 0;
} else {
gyroZero[axis] = (g[axis] + 200) / 399;
blinkLED(10, 15, 1 + 3 * nunchukPresent);
}
}
calibratingG--;
}
#if defined(ITG3200) || defined(L3G4200D)
gyroADC[ROLL] = gyroADC[ROLL] - gyroZero[ROLL];
gyroADC[PITCH] = gyroADC[PITCH] - gyroZero[PITCH];
gyroADC[YAW] = gyroADC[YAW] - gyroZero[YAW];
#else
gyroADC[ROLL] = gyroADC[ROLL] - gyroZero[ROLL];
gyroADC[PITCH] = gyroADC[PITCH] - gyroZero[PITCH];
gyroADC[YAW] = gyroADC[YAW] - gyroZero[YAW];
gyroADC[ROLL] = (rawADC_WMP[3] & 0x01) ? gyroADC[ROLL] / 5 : gyroADC[ROLL] ; //the ratio 1/5 is not exactly the IDG600 or ISZ650 specification
gyroADC[PITCH] = (rawADC_WMP[4] & 0x02) >> 1 ? gyroADC[PITCH] / 5 : gyroADC[PITCH] ; //we detect here the slow of fast mode WMP gyros values (see wiibrew for more details)
gyroADC[YAW] = (rawADC_WMP[3] & 0x02) >> 1 ? gyroADC[YAW] / 5 : gyroADC[YAW] ;
#endif
//anti gyro glitch, limit the variation between two consecutive readings
for (axis = 0; axis < 3; axis++) {
gyroADC[axis] = constrain(gyroADC[axis], previousGyroADC[axis] - 100, previousGyroADC[axis] + 100);
previousGyroADC[axis] = gyroADC[axis];
}
}
if (r == 0 || ( (accPresent == 1) && (withACC == 1) ) ) { //nunchuk or i2c ACC
if (calibratingA > 0) {
if (calibratingA > 1) {
for (uint8_t axis = 0; axis < 3; axis++) {
if (calibratingA == 400) a[axis] = 0;
a[axis] += accADC[axis];
accADC[axis] = 0;
}
} else {
accZero[ROLL] = (a[ROLL] + 200) / 399;
accZero[PITCH] = (a[PITCH] + 200) / 399;
accZero[YAW] = (a[YAW] + 200) / 399 + acc_1G; // for nunchuk 200=1G
writeParams(); // write accZero in EEPROM
}
calibratingA--;
} else {
accADC[ROLL] = accADC[ROLL] - accZero[ROLL] ;
accADC[PITCH] = accADC[PITCH] - accZero[PITCH];
accADC[YAW] = - (accADC[YAW] - accZero[YAW]) ;
}
}
}
return r;
}
void computeIMU () {
uint8_t axis;
static int16_t gyroADCprevious[3] = {0, 0, 0};
static int16_t gyroADCp[3] = {0, 0, 0};
int16_t gyroADCinter[3];
static int16_t lastAccADC[3] = {0, 0, 0};
static int16_t similarNumberAccData[3];
static int16_t gyroDeviation[3];
static uint32_t timeInterleave;
static int16_t gyroYawSmooth = 0;
//we separate the 2 situations because reading gyro values with a gyro only setup can be acchieved at a higher rate
//gyro+nunchuk: we must wait for a quite high delay betwwen 2 reads to get both WM+ and Nunchuk data. It works with 3ms
//gyro only: the delay to read 2 consecutive values can be reduced to only 0.65ms
if (nunchukPresent) {
annexCode();
while ((micros() - timeInterleave) < INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave = micros();
updateIMU(0);
getEstimatedAttitude(); // computation time must last less than one interleaving delay
while ((micros() - timeInterleave) < INTERLEAVING_DELAY) ; //interleaving delay between 2 consecutive reads
timeInterleave = micros();
while (updateIMU(0) != 1) ; // For this interleaving reading, we must have a gyro update at this point (less delay)
for (axis = 0; axis < 3; axis++) {
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADC[axis] * 3 + gyroADCprevious[axis] + 16) / 4 / 8; // /4 is to average 4 values ; /8 is to reduce the sensibility of gyro
gyroADCprevious[axis] = gyroADC[axis];
}
} else {
#if defined(I2C_ACC) || defined(ADCACC)
getEstimatedAttitude();
updateIMU(1); //with I2C or ADC ACC
#else
updateIMU(0); //without ACC
#endif
for (axis = 0; axis < 3; axis++)
gyroADCp[axis] = gyroADC[axis];
timeInterleave = micros();
annexCode();
while ((micros() - timeInterleave) < 650) ; //empirical, interleaving delay between 2 consecutive reads
updateIMU(0); //without ACC
for (axis = 0; axis < 3; axis++) {
gyroADCinter[axis] = gyroADC[axis] + gyroADCp[axis];
// empirical, we take a weighted value of the current and the previous values
gyroData[axis] = (gyroADCinter[axis] + gyroADCprevious[axis] + 12) / 3 / 8; // /3 is to average 3 values ; /8 is to reduce the sensibility of gyro
gyroADCprevious[axis] = gyroADCinter[axis] / 2;
#if not defined (I2C_ACC) && not defined (ADCACC)
accADC[axis] = 0;
#endif
}
}
#if defined(TRI)
gyroData[YAW] = (gyroYawSmooth * 2 + gyroData[YAW] + 1) / 3;
gyroYawSmooth = gyroData[YAW];
#endif
}
// ************************************
// simplified IMU based on Kalman Filter
// inspired from http://starlino.com/imu_guide.html
// and http://www.starlino.com/imu_kalman_arduino.html
// for angles under 25deg, we use an approximation to speed up the angle calculation
// magnetometer addition for small angles
// ************************************
void getEstimatedAttitude() {
uint8_t axis;
float R, RGyro[3]; //R obtained from last estimated value and gyro movement;
static float REst[3] = {0, 0, 1} ; // init acc in stable mode
static float A[2]; //angles between projection of R on XZ/YZ plane and Z axis (in Radian)
float wGyro = 300; // gyro weight/smooting factor
float invW = 1.0 / (1 + 300);
float gyroFactor;
static uint8_t small_angle = 1;
static uint16_t tPrevious;
uint16_t tCurrent, deltaTime;
float a[2], mag[2], cos_[2];
tCurrent = micros();
deltaTime = tCurrent - tPrevious;
tPrevious = tCurrent;
#if defined(ITG3200) || defined(L3G4200D)
gyroFactor = deltaTime / 300e6; //empirical
#else
gyroFactor = deltaTime / 200e6; //empirical, depends on WMP on IDG datasheet, tied of deg/ms sensibility
#endif
for (axis = 0; axis < 2; axis++) a[axis] = gyroADC[axis] * gyroFactor;
for (axis = 0; axis < 3; axis++) accSmooth[axis] = (accSmooth[axis] * 7 + accADC[axis] + 4) / 8;
if (accSmooth[YAW] > 0 ) { //we want to be sure we are not flying inverted
// a very nice trigonometric approximation: under 25deg, the error of this approximation is less than 1 deg:
// sin(x) =~= x =~= arcsin(x)
// angle_axis = arcsin(ACC_axis/ACC_1G) =~= ACC_axis/ACC_1G
// the angle calculation is much more faster in this case
if (accSmooth[ROLL] < acc_25deg && accSmooth[ROLL] > -acc_25deg && accSmooth[PITCH] < acc_25deg && accSmooth[PITCH] > -acc_25deg) {
for (axis = 0; axis < 2; axis++) {
A[axis] += a[axis];
A[axis] = ((float)accSmooth[axis] / acc_1G + A[axis] * wGyro) * invW; // =~= sin axis
#if defined(HMC5843) || defined(HMC5883)
cos_[axis] = 1 - A[axis] * A[axis] / 2; // cos(x) =~= 1-x^2/2
#endif
}
small_angle = 1;
} else {
//magnitude vector size
R = sqrt(square(accSmooth[ROLL]) + square(accSmooth[PITCH]) + square(accSmooth[YAW]));
for (axis = 0; axis < 2; axis++) {
if ( acc_1G * 3 / 5 < R && R < acc_1G * 7 / 5 && small_angle == 0 ) //if accel magnitude >1.4G or <0.6G => we neutralize the effect of accelerometers in the angle estimation
A[axis] = atan2(REst[axis], REst[YAW]);
A[axis] += a[axis];
cos_[axis] = cos(A[axis]);
RGyro[axis] = sin(A[axis]) / sqrt( 1.0 + square(cos_[axis]) * square(tan(A[1 - axis]))); //reverse calculation of RwGyro from Awz angles
}
RGyro[YAW] = sqrt(abs(1.0 - square(RGyro[ROLL]) - square(RGyro[PITCH])));
for (axis = 0; axis < 3; axis++)
REst[axis] = (accADC[axis] / R + wGyro * RGyro[axis]) * invW; //combine Accelerometer and gyro readings
small_angle = 0;
}
#if defined(HMC5843) || defined(HMC5883)
mag[PITCH] = -magADC[PITCH] * cos_[PITCH] + magADC[ROLL] * A[ROLL] * A[PITCH] + magADC[YAW] * cos_[ROLL] * A[PITCH];
mag[ROLL] = magADC[ROLL] * cos_[ROLL] - magADC[YAW] * A[ROLL];
heading = -degrees(atan2(mag[PITCH], mag[ROLL]));
#endif
}
for (axis = 0; axis < 2; axis++) angle[axis] = A[axis] * 572.9577951; //angle in multiple of 0.1 degree
}
// *************************
// motor and servo functions
// *************************
uint8_t PWM_PIN[6] = {MOTOR_ORDER};
static int16_t axisPID[3];
static int16_t motor[6];
static int16_t servo[4] = {1500, 1500, 1500, 1500};
volatile uint8_t atomicServo[4] = {250, 250, 250, 250};
//for HEX Y6 and HEX6/HEX6X flat and for promini
volatile uint8_t atomicPWM_PIN5_lowState;
volatile uint8_t atomicPWM_PIN5_highState;
volatile uint8_t atomicPWM_PIN6_lowState;
volatile uint8_t atomicPWM_PIN6_highState;
void writeMotors() { // [1000;2000] => [125;250]
for (uint8_t i = 0; i < min(NUMBER_MOTOR, 4); i++)
analogWrite(PWM_PIN[i], motor[i] >> 3);
#if (NUMBER_MOTOR == 6) && defined(MEGA)
analogWrite(PWM_PIN[4], motor[4] >> 3);
analogWrite(PWM_PIN[5], motor[5] >> 3);
#endif
#if (NUMBER_MOTOR == 6) && defined(PROMINI)
atomicPWM_PIN5_highState = motor[5] / 8;
atomicPWM_PIN5_lowState = 255 - atomicPWM_PIN5_highState;
atomicPWM_PIN6_highState = motor[4] / 8;
atomicPWM_PIN6_lowState = 255 - atomicPWM_PIN6_highState;
#endif
}
void writeAllMotors(int16_t mc) { // Sends commands to all motors
for (uint8_t i = 0; i < NUMBER_MOTOR; i++)
motor[i] = mc;
writeMotors();
}
void initializeMotors() {
for (uint8_t i = 0; i < NUMBER_MOTOR; i++)
pinMode(PWM_PIN[i], OUTPUT);
writeAllMotors(1000);
delay(300);
}
#if defined(SERVO)
void initializeServo() {
#if defined(TRI)
DIGITAL_SERVO_TRI_PINMODE
#endif
#if defined(SERVO_TILT) || defined(GIMBAL) || defined(FLYING_WING)
DIGITAL_TILT_ROLL_PINMODE
DIGITAL_TILT_PITCH_PINMODE
#endif
#if defined(CAMTRIG)
DIGITAL_CAM_PINMODE
#endif
#if defined(BI)
DIGITAL_SERVO_TRI_PINMODE
DIGITAL_BI_LEFT_PINMODE
#endif
TCCR0A = 0; // normal counting mode
TIMSK0 |= (1 << OCIE0A); // Enable CTC interrupt
}
......................split here......................................
I say I think because there are 2 folders in the ZIP file, MultiWiiConf1_7 and MultiWii1_7. The later has an old style .PDE file in it (the above code) and the former has another .PDE file and apparently an executable to run on the PC. I assume these are to configure the gains and constants and etc, for the V1_7 code.
Trying to compile the above w/the target board being an Uno gets me these error messages :
C:\Users\Temp\build7033488152536874740.tmp/core.a(HardwareSerial0.cpp.o): In function `__vector_19':
C:\Program Files (x86)\Arduino\hardware\arduino\avr\cores\arduino/HardwareSerial0.cpp:59: multiple definition of `__vector_19'
C:\Users\\Temp\build7033488152536874740.tmp\sketch_sep24a.cpp.o:C:\Program Files (x86)\Arduino/sketch_sep24a.ino:2216: first defined here
Line 2216 deals with register UDR0, which game me more direct problems when the target was a 32U4 based board (vs the 328p based Uno).
@ the OP : Is this what you’re seeing ? It sounds different.