Subversion Repositories FlightCtrl

/*********************************************************************************/

/* Attitude sense system (processing of gyro, accelerometer and altimeter data)  */

/*********************************************************************************/

#ifndef _ATTITUDE_H

#define _ATTITUDE_H

#include <inttypes.h>

#include "analog.h"

// For debugging only.

#include "uart0.h"

/*

 * If you have no acc. sensor or do not want to use it, remove this define. This will cause the

 * acc. sensor to be ignored at attitude calibration.

 */

#define ATTITUDE_USE_ACC_SENSORS yes

/*

 * Default hysteresis to use for the -180 degree to 180 degree wrap.

 */

#define PITCHOVER_HYSTERESIS 0L

#define ROLLOVER_HYSTERESIS 0L

/*

 * The frequency at which numerical integration takes place. 488 in original code.

 */

#define INTEGRATION_FREQUENCY 488

/*

 * Gyro readings are divided by this before being used in attitude control. This will scale them

 * to match the scale of the stick control etc. variables. This is just a rough non-precision

 * scaling - the below definitions make precise conversion factors.

 */

#define HIRES_GYRO_INTEGRATION_FACTOR 1

// (int)((GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION) / 1250)

/*

 Gyro integration:

 The control loop executes at INTEGRATION_FREQUENCY Hz, 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 =

 t * INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR

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

 gyroIntegral = INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR

 This number (INTEGRATION_FREQUENCY * v * GYRO_RATE_FACTOR_PITCHROLL / HIRES_GYRO_INTEGRATION_FACTOR) is the integral-to-degree factor.

 Examples:

 FC1.3:                 GYRO_DEG_FACTOR_PITCHROLL = 2545

 FC2.0:                 GYRO_DEG_FACTOR_PITCHROLL = 2399

 My InvenSense copter:  GYRO_DEG_FACTOR_PITCHROLL = 1333

 */

#define GYRO_DEG_FACTOR_PITCHROLL (uint16_t)(GYRO_RATE_FACTOR_PITCHROLL * INTEGRATION_FREQUENCY * GYRO_PITCHROLL_CORRECTION / HIRES_GYRO_INTEGRATION_FACTOR)

#define GYRO_DEG_FACTOR_YAW (uint16_t)(GYRO_RATE_FACTOR_YAW * INTEGRATION_FREQUENCY * GYRO_YAW_CORRECTION)

/*

 * Constant for deriving an attitude angle from acceleration measurement.

 *

 * The value is derived from the datasheet of the ACC sensor where 5g are scaled to VRef.

 * 1g is (3V * 1024) / (5 * 3V) = 205 counts. The ADC ISR sums 2 acc. samples to each

 * [pitch/roll]AxisAcc and thus reads about acc = 410 counts / g.

 * We approximate a small pitch/roll angle v by assuming that the copter does not accelerate:

 * In this explanation it is assumed that the ADC values are 0 based, and gravity is included.

 * The sine of v is the far side / the hypothenusis:

 * sin v = acc / sqrt(acc^2 + acc_z^2)

 * Using that v is a small angle, and the near side is about equal to the the hypothenusis:

 * sin v ~= acc / acc_z

 * Assuming that the helicopter is hovering at small pitch and roll angles, acc_z is about 410,

 * and sin v ~= v (small angles, in radians):

 * sin v ~= acc / 410

 * v / 57.3 ~= acc / 410

 * v ~= acc * 57.3 / 410

 * acc / v ~= 410 / 57.3 ~= 7, that is: There are about 7 counts per degree.

 *

 * Summary: DEG_ACC_FACTOR = (2 * 1024 * [sensitivity of acc. meter in V/g]) / (3V * 57.3)

 */

#define DEG_ACC_FACTOR 7

/*

 * This is ([gyro integral value] / degree) / (degree / acc. sensor value) = gyro integral value / acc.sensor value

 * = the factor an acc. sensor should be multiplied by to get the gyro integral

 * value for the same (small) angle.

 * The value is about 200.

 */

#define GYRO_ACC_FACTOR ((GYRO_DEG_FACTOR_PITCHROLL) / (DEG_ACC_FACTOR))

#define PITCHROLLOVER180 (GYRO_DEG_FACTOR_PITCHROLL * 180L)

#define PITCHROLLOVER360 (GYRO_DEG_FACTOR_PITCHROLL * 360L)

#define YAWOVER180       (GYRO_DEG_FACTOR_YAW * 180L)

#define YAWOVER360       (GYRO_DEG_FACTOR_YAW * 360L)

/*

 * Rotation rates

 */

extern int16_t rate_PID[2], rate_ATT[2], yawRate;

extern int16_t differential[2];

/*

 * Attitudes calculated by numerical integration of gyro rates

 */

extern int32_t attitude[2];

// This is really a flight module thing, but it should be corrected along

// when the yaw angle is corrected from the compass, and that happens here.

// extern int32_t yawAngleDiff;

/*

 * Compass navigation

 */

extern int16_t magneticHeading;

//extern int16_t headingInDegrees;

extern int32_t heading;

extern uint16_t ignoreCompassTimer;

extern uint16_t accVector;

extern int32_t targetHeading;

/*

 * Dynamic gyro offsets. These are signed values that are subtracted from the gyro measurements,

 * to help canceling out drift and vibration noise effects. The dynamic offsets themselves

 * can be updated in flight by different ways, for example:

 * - Just taking them from parameters, so the pilot can trim manually in a PC or mobile tool

 * - Summing up how much acc. meter correction was done to the gyro integrals over the last n

 *   integration, and then adding the sum / n to the dynamic offset

 * - Detect which way the pilot pulls the stick to keep the copter steady, and correct by that

 * - Invent your own...

 */

extern int16_t dynamicOffset[2], dynamicOffsetYaw;

/*

 * For NaviCtrl use.

 */

extern int16_t averageAcc[2], averageAccCount;

/*

 * Re-init flight attitude, setting all angles to 0 (or to whatever can be derived from acc. sensor).

 * To be called when the pilot commands gyro calibration (eg. by moving the left stick up-left or up-right).

 */

void attitude_setNeutral(void);

/*

 * Experiment.

 */

// void attitude_startDynamicCalibration(void);

// void attitude_continueDynamicCalibration(void);

int32_t getAngleEstimateFromAcc(uint8_t axis);

/*

 * Main routine, called from the flight loop.

 */

void calculateFlightAttitude(void);

void attitude_resetHeadingToMagnetic(void);

#endif //_ATTITUDE_H