Subversion Repositories FlightCtrl

#ifndef _ANALOG_H

#define _ANALOG_H

#include <inttypes.h>

// #include "invenSense.h"

#include "ENC-03_FC1.3.h"

/*

 * How much low pass filtering to apply for gyro_PID.

 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...

 * Temporarily replaced by userparam-configurable variable.

 */

#define GYROS_PIDFILTER 1

/*

 * How much low pass filtering to apply for gyro_ATT.

 * 0=illegal, 1=no filtering, 2=50% last value + 50% new value, 3=67% last value + 33 % new value etc...

 * Temporarily replaced by userparam-configurable variable.

 */

#define GYROS_INTEGRALFILTER 1

// Temporarily replaced by userparam-configurable variable.

//#define ACC_FILTER 4

/*

  About setting constants for different gyros:

  Main parameters are positive directions and voltage/angular speed gain.

  The "Positive direction" is the rotation direction around an axis where

  the corresponding gyro outputs a voltage > the no-rotation voltage.

  A gyro is considered, in this code, to be "forward" if its positive

  direction is the same as in FC1.0-1.3, and reverse otherwise.

  Declare the GYRO_REVERSE_YAW, GYRO_REVERSE_ROLL and

  GYRO_REVERSE_PITCH #define's if the respective gyros are reverse.

  Setting gyro gain correctly: All sensor measurements in analog.c take

  place in a cycle, each cycle comprising all sensors. Some sensors are

  sampled more than ones, and the results added. The pitch and roll gyros

  are sampled 4 times and the yaw gyro 2 times in the original H&I V0.74

  code.

  In the H&I code, the results for pitch and roll are multiplied by 2 (FC1.0)

  or 4 (other versions), offset to zero, low pass filtered and then assigned

  to the "HiResXXXX" and "AdWertXXXXFilter" variables, where XXXX is nick or

  roll.

  So:

  gyro = V * (ADCValue1 + ADCValue2 + ADCValue3 + ADCValue4),

    where V is 2 for FC1.0 and 4 for all others.

  Assuming constant ADCValue, in the H&I code:

  gyro = I * ADCValue.

  where I is 8 for FC1.0 and 16 for all others.

  The relation between rotation rate and ADCValue:

  ADCValue [units] =

    rotational speed [deg/s] *

    gyro sensitivity [V / deg/s] *

    amplifier gain [units] *

    1024 [units] /

    3V full range [V]

  or: H is the number of steps the ADC value changes with,

  for a 1 deg/s change in rotational velocity:

  H = ADCValue [units] / rotation rate [deg/s] =

    gyro sensitivity [V / deg/s] *

    amplifier gain [units] *

    1024 [units] /

    3V full range [V]

  Examples:

  FC1.3 has 0.67 mV/deg/s gyros and amplifiers with a gain of 5.7:

    H = 0.00067 V / deg / s * 5.7 * 1024 / 3V = 1.304 units/(deg/s).

  FC2.0 has 6*(3/5) mV/deg/s gyros (they are ratiometric) and no amplifiers:

    H = 0.006 V / deg / s * 1 * 1024 * 3V / (3V * 5V) = 1.2288 units/(deg/s).

  My InvenSense copter has 2mV/deg/s gyros and no amplifiers:

    H = 0.002 V / deg / s * 1 * 1024 / 3V = 0.6827 units/(deg/s)

    (only about half as sensitive as V1.3. But it will take about twice the

     rotation rate!)

  All together: gyro = I * H * rotation rate [units / (deg/s)].

*/

/*

 * A factor that the raw gyro values are multiplied by,

 * before being filtered and passed to the attitude module.

 * A value of 1 would cause a little bit of loss of precision in the

 * filtering (on the other hand the values are so noisy in flight that

 * it will not really matter - but when testing on the desk it might be

 * noticeable). 4 is fine for the default filtering.

 * Experiment: Set it to 1.

 */

#define GYRO_FACTOR_PITCHROLL 4

/*

 * How many samples are summed in one ADC loop, for pitch&roll and yaw,

 * respectively. This is = the number of occurences of each channel in the

 * channelsForStates array in analog.c.

 */

#define GYRO_SUMMATION_FACTOR_PITCHROLL 4

#define GYRO_SUMMATION_FACTOR_YAW 2

/*

  Integration:

  The HiResXXXX values are divided by 8 (in H&I firmware) before integration.

  In the Killagreg rewrite of the H&I firmware, the factor 8 is called

  HIRES_GYRO_AMPLIFY. In this code, it is called HIRES_GYRO_INTEGRATION_FACTOR,

  and care has been taken that all other constants (gyro to degree factor, and

  180 degree flip-over detection limits) are corrected to it. Because the

  division by the constant takes place in the flight attitude code, the

  constant is there.

  The control loop executes every 2ms, and for each iteration

  gyro_ATT[PITCH/ROLL] is added to gyroIntegral[PITCH/ROLL].

  Assuming a constant rotation rate v and a zero initial gyroIntegral

  (for this explanation), we get:

  gyroIntegral =

    N * gyro / HIRES_GYRO_INTEGRATION_FACTOR =

    N * I * H * v / HIRES_GYRO_INTEGRATION_FACTOR

  where N is the number of summations; N = t/2ms.

  For one degree of rotation: t*v = 1:

  gyroIntegralXXXX = t/2ms * I * H * 1/t = INTEGRATION_FREQUENCY * I * H / HIRES_GYRO_INTEGRATION_FACTOR.

  This number (INTEGRATION_FREQUENCY * I * H) is the integral-to-degree factor.

  Examples:

  FC1.3: I=2, H=1.304, HIRES_GYRO_INTEGRATION_FACTOR=8 --> integralDegreeFactor = 1304

  FC2.0: I=2, H=2.048, HIRES_GYRO_INTEGRATION_FACTOR=13 --> integralDegreeFactor = 1260

  My InvenSense copter: HIRES_GYRO_INTEGRATION_FACTOR=4, H=0.6827 --> integralDegreeFactor = 1365

*/

/*

 * The value of gyro[PITCH/ROLL] for one deg/s = The hardware factor H * the number of samples * multiplier factor.

 * Will be about 10 or so for InvenSense, and about 33 for ADXRS610.

 */

#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_PITCHROLL * GYRO_FACTOR_PITCHROLL)

#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_SUMMATION_FACTOR_YAW)

/*

 * Gyro saturation prevention.

 */

// How far from the end of its range a gyro is considered near-saturated.

#define SENSOR_MIN_PITCHROLL 32

// Other end of the range (calculated)

#define SENSOR_MAX_PITCHROLL (GYRO_SUMMATION_FACTOR_PITCHROLL * 1023 - SENSOR_MIN_PITCHROLL)

// Max. boost to add "virtually" to gyro signal at total saturation.

#define EXTRAPOLATION_LIMIT 2500

// Slope of the boost (calculated)

#define EXTRAPOLATION_SLOPE (EXTRAPOLATION_LIMIT/SENSOR_MIN_PITCHROLL)

/*

 * This value is subtracted from the gyro noise measurement in each iteration,

 * making it return towards zero.

 */

#define GYRO_NOISE_MEASUREMENT_DAMPING 5

#define PITCH 0

#define ROLL 1

/*

 * The values that this module outputs

 * These first 2 exported arrays are zero-offset. The "PID" ones are used

 * in the attitude control as rotation rates. The "ATT" ones are for

 * integration to angles. For the same axis, the PID and ATT variables

 * generally have about the same values. There are just some differences

 * in filtering, and when a gyro becomes near saturated.

 * Maybe this distinction is not really necessary.

 */

extern volatile int16_t gyro_PID[2];

extern volatile int16_t gyro_ATT[2];

extern volatile int16_t gyroD[2];

extern volatile uint16_t ADCycleCount;

extern volatile int16_t UBat;

extern volatile int16_t yawGyro;

/*

 * This is not really for external use - but the ENC-03 gyro modules needs it.

 */

extern volatile int16_t rawGyroSum[2], rawYawGyroSum;

/*

 * The acceleration values that this module outputs. They are zero based.

 */

extern volatile int16_t acc[2], ZAcc;

extern volatile int16_t filteredAcc[2];

/*

 * Flag: Interrupt handler has done all A/D conversion and processing.

 */

extern volatile uint8_t analogDataReady;

// Diagnostics: Gyro noise level because of motor vibrations. The variables

// only really reflect the noise level when the copter stands still but with 

// its motors running.

extern volatile uint16_t gyroNoisePeak[2];

extern volatile uint16_t accNoisePeak[2];

// void SearchAirPressureOffset(void);

void analog_init(void);

// clear ADC enable & ADC Start Conversion & ADC Interrupt Enable bit

#define analog_stop() (ADCSRA &= ~((1<<ADEN)|(1<<ADSC)|(1<<ADIE)))

// set ADC enable & ADC Start Conversion & ADC Interrupt Enable bit

#define analog_start() (ADCSRA |= (1<<ADEN)|(1<<ADSC)|(1<<ADIE))

/*

 * "Warm" calibration: Zero-offset gyros.

 */

void analog_calibrate(void);

/*

 * "Cold" calibration: Zero-offset accelerometers and write the calibration data to EEPROM.

 */

void analog_calibrateAcc(void);

#endif //_ANALOG_H