6,141 → 6,23 |
#define _ATTITUDE_H |
|
#include <inttypes.h> |
#include <math.h> |
|
#include "analog.h" |
#include "timer0.h" |
//#include "timer0.h" |
//#define _PI 3.1415926535897932384626433832795 |
|
/* |
* 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 |
#define ACC_MS2_FACTOR (1.0f / ACC_G_FACTOR_XY) |
|
/* |
* Default hysteresis to use for the -180 degree to 180 degree wrap. |
*/ |
#define PITCHOVER_HYSTERESIS 0L |
#define ROLLOVER_HYSTERESIS 0L |
// 57.3 |
#define RAD_DEG_FACTOR (180.0 / M_PI) |
|
/* |
* The frequency at which numerical integration takes place. 488 in original code. |
*/ |
#define INTEGRATION_FREQUENCY F_MAINLOOP |
|
/* |
* 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_PITCHROLL_CORRECTION GYRO_PITCHROLL_CORRECTION_should_be_overridden_with_a_-D_at_compile_time |
#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 multicopter 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]; |
extern int16_t attitude[3]; |
|
// 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 int32_t heading; |
extern uint16_t ignoreCompassTimer; |
extern uint16_t accVector; |
|
extern int32_t headingError; |
|
|
/* |
* 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). |
*/ |
147,18 → 29,8 |
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_update(void); |
|
void attitude_resetHeadingToMagnetic(void); |
|
#endif //_ATTITUDE_H |