Subversion Repositories FlightCtrl

Rev

Rev 2109 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
2108 - 1
#include <stdlib.h>
2
#include <avr/io.h>
3
#include <stdlib.h>
4
 
5
#include "attitude.h"
6
#include "dongfangMath.h"
7
#include "commands.h"
8
 
9
// For scope debugging only!
10
#include "rc.h"
11
 
12
// where our main data flow comes from.
13
#include "analog.h"
14
 
15
#include "configuration.h"
16
#include "output.h"
17
 
18
// Some calculations are performed depending on some stick related things.
19
#include "controlMixer.h"
20
 
21
#define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;}
22
 
23
/*
24
 * Gyro readings, as read from the analog module. It would have been nice to flow
25
 * them around between the different calculations as a struct or array (doing
26
 * things functionally without side effects) but this is shorter and probably
27
 * faster too.
28
 * The variables are overwritten at each attitude calculation invocation - the values
29
 * are not preserved or reused.
30
 */
31
 
32
/*
33
 * Gyro integrals. These are the rotation angles of the airframe compared to the
34
 * horizontal plane, yaw relative to yaw at start.
35
 */
36
int32_t attitude[3];
37
 
38
/*
39
 * Experiment: Compensating for dynamic-induced gyro biasing.
40
 */
41
int16_t driftComp[3] = { 0, 0, 0 };
42
// int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0;
43
// int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw;
44
// int16_t dynamicCalCount;
45
 
46
/************************************************************************
47
 * Set inclination angles from the acc. sensor data.                    
48
 * If acc. sensors are not used, set to zero.                          
49
 * TODO: One could use inverse sine to calculate the angles more        
50
 * accurately, but since: 1) the angles are rather small at times when
51
 * it makes sense to set the integrals (standing on ground, or flying at  
52
 * constant speed, and 2) at small angles a, sin(a) ~= constant * a,    
53
 * it is hardly worth the trouble.                                      
54
 ************************************************************************/
55
/*
56
int32_t getAngleEstimateFromAcc(uint8_t axis) {
57
  return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
58
}
59
*/
60
 
61
void setStaticAttitudeAngles(void) {
2109 - 62
  attitude[PITCH] = attitude[ROLL] = attitude[YAW] = 0;
2108 - 63
}
64
 
65
/************************************************************************
66
 * Neutral Readings                                                    
67
 ************************************************************************/
68
void attitude_setNeutral(void) {
69
    // Calibrate hardware.
70
  analog_setNeutral();
71
 
72
  // reset gyro integrals to acc guessing
73
  setStaticAttitudeAngles();
74
}
75
 
76
/************************************************************************
77
 * Get sensor data from the analog module, and release the ADC          
78
 * TODO: Ultimately, the analog module could do this (instead of dumping
79
 * the values into variables).
80
 * The rate variable end up in a range of about [-1024, 1023].
81
 *************************************************************************/
82
void getAnalogData(void) {
83
  analog_update();
84
}
85
 
86
void integrate(void) {
87
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
88
  uint8_t axis;
89
 
90
  /*
91
   * Yaw
92
   * Calculate yaw gyro integral (~ to rotation angle)
93
   * Limit heading proportional to 0 deg to 360 deg
94
   */
95
  /*
96
   * Pitch axis integration and range boundary wrap.
97
   */
98
 
99
  for (axis = PITCH; axis <= YAW; axis++) {
100
    attitude[axis] += gyro_ATT[axis];
101
    if (attitude[axis] > OVER180) {
102
      attitude[axis] -= OVER360;
103
    } else if (attitude[axis] <= -OVER180) {
104
      attitude[axis] += OVER360;
105
    }
106
  }
107
}
108
 
109
/************************************************************************
110
 * Main procedure.
111
 ************************************************************************/
112
void calculateFlightAttitude(void) {
113
  getAnalogData();
114
  integrate();
115
 
116
  // We are done reading variables from the analog module.
117
  // Interrupt-driven sensor reading may restart.
118
  startAnalogConversionCycle();
119
}