Subversion Repositories FlightCtrl

#ifndef _ANALOG_H

#define _ANALOG_H

#include <inttypes.h>

#include "configuration.h"

/*

 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:

 - Nose down for pitch

 - Left hand side down for roll

 - Clockwise seen from above for yaw.

 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 once (oversampled), and the results added.

 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. The factor 2 or 4 or whatever is called GYRO_FACTOR_PITCHROLL here.

*/

#define GYRO_FACTOR_PITCHROLL 1

/*

 GYRO_HW_FACTOR is 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]

 GYRO_HW_FACTOR is:

 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:

 GYRO_HW_FACTOR = 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:

 GYRO_HW_FACTOR = 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:

 GYRO_HW_FACTOR = 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!)

 GYRO_HW_FACTOR is given in the makefile.

*/

/*

 * How many samples are added 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_OVERSAMPLING_PITCHROLL 4

#define GYRO_OVERSAMPLING_YAW 2

#define ACC_OVERSAMPLING_XY 2

#define ACC_OVERSAMPLING_Z 1

/*

 * The product of the 3 above constants. This represents the expected change in ADC value sums for 1 deg/s of rotation rate.

 */

#define GYRO_RATE_FACTOR_PITCHROLL (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_PITCHROLL * GYRO_FACTOR_PITCHROLL)

#define GYRO_RATE_FACTOR_YAW (GYRO_HW_FACTOR * GYRO_OVERSAMPLING_YAW)

/*

 * 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.

 */

/*

 * 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_OVERSAMPLING_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

#define YAW 2

#define Z 2

/*

 * 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 int16_t gyro_PID[2];

extern int16_t gyro_ATT[2];

#define GYRO_D_WINDOW_LENGTH 3

extern int16_t gyroD[2];

extern int16_t yawGyro;

extern volatile uint16_t ADCycleCount;

extern int16_t UBat;

// 1:11 voltage divider, 1024 counts per 3V, and result is divided by 3.

#define UBAT_AT_5V (int16_t)((5.0 * (1.0/11.0)) * 1024 / (3.0 * 3))

extern sensorOffset_t gyroOffset;

extern sensorOffset_t accOffset;

extern sensorOffset_t gyroAmplifierOffset;

/*

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

 */

//extern volatile int16_t rawGyroSum[3];

/*

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

 */

extern int16_t acc[3];

extern int16_t filteredAcc[3];

// extern volatile int32_t stronglyFilteredAcc[3];

/*

 * 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 uint16_t gyroNoisePeak[3];

extern uint16_t accNoisePeak[3];

/*

 * Air pressure.

 * The sensor has a sensitivity of 45 mV/kPa.

 * An approximate p(h) formula is = p(h[m])[kPa] = p_0 - 11.95 * 10^-3 * h

 * p(h[m])[kPa] = 101.3 - 11.95 * 10^-3 * h

 * 11.95 * 10^-3 * h = 101.3 - p[kPa]

 * h = (101.3 - p[kPa])/0.01195

 * That is: dV = -45 mV * 11.95 * 10^-3 dh = -0.53775 mV / m.

 * That is, with 38.02 * 1.024 / 3 steps per mV: -7 steps / m

Display pressures

4165 mV-->1084.7

4090 mV-->1602.4   517.7

3877 mV-->3107.8  1503.4

4165 mV-->1419.1

3503 mV-->208.1

Diff.:   1211.0

Calculated  Vout = 5V(.009P-0.095) --> 5V .009P = Vout + 5V 0.095 --> P = (Vout + 5V 0.095)/(5V 0.009)

4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m

4090 mV = 5V(0.009P-0.095)  P = 101.44 kPa  h = -11.7m   139.7m

3877 mV = 5V(0.009P-0.095)  P = 96.7   kPa  h = 385m     396.7m

4165 mV = 5V(0.009P-0.095)  P = 103.11 kPa  h = -151.4m

3503 mV = 5V(0.009P-0.095)  P = 88.4   kPa  h = 384m  Diff: 1079.5m

Pressure at sea level: 101.3 kPa. voltage: 5V * (0.009P-0.095) = 4.0835V

This is OCR2 = 143.15 at 1.5V in --> simple pressure =

*/

#define AIRPRESSURE_OVERSAMPLING 14

#define AIRPRESSURE_FILTER 8

// Minimum A/D value before a range change is performed.

#define MIN_RAWPRESSURE (200 * 2)

// Maximum A/D value before a range change is performed.

#define MAX_RAWPRESSURE (1023 * 2 - MIN_RAWPRESSURE)

#define MIN_RANGES_EXTRAPOLATION 15

#define MAX_RANGES_EXTRAPOLATION 240

#define PRESSURE_EXTRAPOLATION_COEFF 25L

#define AUTORANGE_WAIT_FACTOR 1

#define ABS_ALTITUDE_OFFSET 108205

extern uint16_t simpleAirPressure;

/*

 * At saturation, the filteredAirPressure may actually be (simulated) negative.

 */

extern int32_t filteredAirPressure;

extern int16_t magneticHeading;

extern uint32_t gyroActivity;

/*

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

 */

extern volatile uint8_t analogDataReady;

void analog_init(void);

/*

 * This is really only for use for the ENC-03 code module, which needs to get the raw value

 * for its calibration. The raw value should not be used for anything else.

 */

uint16_t rawGyroValue(uint8_t axis);

/*

 * Start the conversion cycle. It will stop automatically.

 */

void startAnalogConversionCycle(void);

/*

 * Process the sensor data to update the exported variables. Must be called after each measurement cycle and before the data is used.

 */

void analog_update(void);

/*

 * Read gyro and acc.meter calibration from EEPROM.

 */

void analog_setNeutral(void);

/*

 * Zero-offset gyros and write the calibration data to EEPROM.

 */

void analog_calibrateGyros(void);

/*

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

 */

void analog_calibrateAcc(void);

void analog_setGround(void);

int32_t analog_getHeight(void);

int16_t analog_getDHeight(void);

#endif //_ANALOG_H