Rev 2110 | 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) { |
||
| 2099 | - | 83 | analog_update(); |
| 1910 | - | 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 | /* |
||
| 2099 | - | 91 | * Yaw |
| 92 | * Calculate yaw gyro integral (~ to rotation angle) |
||
| 93 | * Limit heading proportional to 0 deg to 360 deg |
||
| 94 | */ |
||
| 95 | /* |
||
| 1910 | - | 96 | * Pitch axis integration and range boundary wrap. |
| 97 | */ |
||
| 1927 | - | 98 | |
| 2099 | - | 99 | for (axis = PITCH; axis <= YAW; axis++) { |
| 2103 | - | 100 | attitude[axis] += gyro_ATT[axis]; |
| 2099 | - | 101 | if (attitude[axis] > OVER180) { |
| 102 | attitude[axis] -= OVER360; |
||
| 103 | } else if (attitude[axis] <= -OVER180) { |
||
| 104 | attitude[axis] += OVER360; |
||
| 1927 | - | 105 | } |
| 1910 | - | 106 | } |
| 107 | } |
||
| 108 | |||
| 109 | /************************************************************************ |
||
| 110 | * Main procedure. |
||
| 111 | ************************************************************************/ |
||
| 112 | void calculateFlightAttitude(void) { |
||
| 113 | getAnalogData(); |
||
| 114 | integrate(); |
||
| 115 | |||
| 2099 | - | 116 | // We are done reading variables from the analog module. |
| 117 | // Interrupt-driven sensor reading may restart. |
||
| 118 | startAnalogConversionCycle(); |
||
| 1910 | - | 119 | } |