Subversion Repositories FlightCtrl

Rev

Rev 2103 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1910 - 1
#include <stdlib.h>
2
#include <avr/io.h>
2099 - 3
#include <stdlib.h>
1910 - 4
 
5
#include "attitude.h"
6
#include "dongfangMath.h"
2099 - 7
#include "commands.h"
1910 - 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
/*
1922 - 33
 * Gyro integrals. These are the rotation angles of the airframe compared to the
2099 - 34
 * horizontal plane, yaw relative to yaw at start.
1910 - 35
 */
2099 - 36
int32_t attitude[3];
1910 - 37
 
38
/*
39
 * Experiment: Compensating for dynamic-induced gyro biasing.
40
 */
2099 - 41
int16_t driftComp[3] = { 0, 0, 0 };
1910 - 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
 ************************************************************************/
2099 - 55
/*
1910 - 56
int32_t getAngleEstimateFromAcc(uint8_t axis) {
2099 - 57
  return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis];
1910 - 58
}
2099 - 59
*/
1910 - 60
 
61
void setStaticAttitudeAngles(void) {
2110 - 62
  attitude[PITCH] = attitude[ROLL] = attitude[YAW] = 0;
1910 - 63
}
64
 
65
/************************************************************************
66
 * Neutral Readings                                                    
67
 ************************************************************************/
68
void attitude_setNeutral(void) {
2099 - 69
    // Calibrate hardware.
70
  analog_setNeutral();
1910 - 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
  uint8_t axis;
2099 - 84
  analog_update();
85
  for (axis = PITCH; axis <= YAW; axis++) {
1910 - 86
  }
87
}
88
 
89
void integrate(void) {
90
  // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate.
91
  uint8_t axis;
92
 
93
  /*
2099 - 94
   * Yaw
95
   * Calculate yaw gyro integral (~ to rotation angle)
96
   * Limit heading proportional to 0 deg to 360 deg
97
   */
98
  /*
1910 - 99
   * Pitch axis integration and range boundary wrap.
100
   */
1927 - 101
 
2099 - 102
  for (axis = PITCH; axis <= YAW; axis++) {
2103 - 103
    attitude[axis] += gyro_ATT[axis];
2099 - 104
    if (attitude[axis] > OVER180) {
105
      attitude[axis] -= OVER360;
106
    } else if (attitude[axis] <= -OVER180) {
107
      attitude[axis] += OVER360;
1927 - 108
    }
1910 - 109
  }
110
}
111
 
112
/************************************************************************
113
 * Main procedure.
114
 ************************************************************************/
115
void calculateFlightAttitude(void) {
116
  getAnalogData();
117
  integrate();
118
 
2099 - 119
  // We are done reading variables from the analog module.
120
  // Interrupt-driven sensor reading may restart.
121
  startAnalogConversionCycle();
1910 - 122
}