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 | } |