Rev 2024 | 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 | */ |
||
| 2099 | - | 31 | int16_t rate_ATT[3]; |
| 1910 | - | 32 | |
| 33 | // With different (less) filtering |
||
| 2099 | - | 34 | int16_t rate_PID[3]; |
| 1910 | - | 35 | int16_t differential[3]; |
| 36 | |||
| 37 | /* |
||
| 1922 | - | 38 | * Gyro integrals. These are the rotation angles of the airframe compared to the |
| 2099 | - | 39 | * horizontal plane, yaw relative to yaw at start. |
| 1910 | - | 40 | */ |
| 2099 | - | 41 | int32_t attitude[3]; |
| 1910 | - | 42 | |
| 43 | /* |
||
| 44 | * Experiment: Compensating for dynamic-induced gyro biasing. |
||
| 45 | */ |
||
| 2099 | - | 46 | int16_t driftComp[3] = { 0, 0, 0 }; |
| 1910 | - | 47 | // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
| 48 | // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
||
| 49 | // int16_t dynamicCalCount; |
||
| 50 | |||
| 51 | /************************************************************************ |
||
| 52 | * Set inclination angles from the acc. sensor data. |
||
| 53 | * If acc. sensors are not used, set to zero. |
||
| 54 | * TODO: One could use inverse sine to calculate the angles more |
||
| 55 | * accurately, but since: 1) the angles are rather small at times when |
||
| 56 | * it makes sense to set the integrals (standing on ground, or flying at |
||
| 57 | * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
||
| 58 | * it is hardly worth the trouble. |
||
| 59 | ************************************************************************/ |
||
| 2099 | - | 60 | /* |
| 1910 | - | 61 | int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| 2099 | - | 62 | return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
| 1910 | - | 63 | } |
| 2099 | - | 64 | */ |
| 1910 | - | 65 | |
| 66 | void setStaticAttitudeAngles(void) { |
||
| 2099 | - | 67 | attitude[PITCH] = attitude[ROLL] = 0; |
| 1910 | - | 68 | } |
| 69 | |||
| 70 | /************************************************************************ |
||
| 71 | * Neutral Readings |
||
| 72 | ************************************************************************/ |
||
| 73 | void attitude_setNeutral(void) { |
||
| 2099 | - | 74 | // Calibrate hardware. |
| 75 | analog_setNeutral(); |
||
| 1910 | - | 76 | |
| 77 | // reset gyro integrals to acc guessing |
||
| 78 | setStaticAttitudeAngles(); |
||
| 79 | } |
||
| 80 | |||
| 81 | /************************************************************************ |
||
| 82 | * Get sensor data from the analog module, and release the ADC |
||
| 83 | * TODO: Ultimately, the analog module could do this (instead of dumping |
||
| 84 | * the values into variables). |
||
| 85 | * The rate variable end up in a range of about [-1024, 1023]. |
||
| 86 | *************************************************************************/ |
||
| 87 | void getAnalogData(void) { |
||
| 88 | uint8_t axis; |
||
| 89 | |||
| 2099 | - | 90 | analog_update(); |
| 91 | |||
| 92 | for (axis = PITCH; axis <= YAW; axis++) { |
||
| 93 | rate_PID[axis] = gyro_PID[axis]; |
||
| 94 | rate_ATT[axis] = gyro_ATT[axis]; |
||
| 1910 | - | 95 | differential[axis] = gyroD[axis]; |
| 96 | } |
||
| 97 | } |
||
| 98 | |||
| 99 | void integrate(void) { |
||
| 100 | // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
||
| 101 | uint8_t axis; |
||
| 102 | |||
| 103 | /* |
||
| 2099 | - | 104 | * Yaw |
| 105 | * Calculate yaw gyro integral (~ to rotation angle) |
||
| 106 | * Limit heading proportional to 0 deg to 360 deg |
||
| 107 | */ |
||
| 108 | /* |
||
| 1910 | - | 109 | * Pitch axis integration and range boundary wrap. |
| 110 | */ |
||
| 1927 | - | 111 | |
| 2099 | - | 112 | for (axis = PITCH; axis <= YAW; axis++) { |
| 113 | attitude[axis] += rate_ATT[axis]; |
||
| 114 | if (attitude[axis] > OVER180) { |
||
| 115 | attitude[axis] -= OVER360; |
||
| 116 | } else if (attitude[axis] <= -OVER180) { |
||
| 117 | attitude[axis] += OVER360; |
||
| 1927 | - | 118 | } |
| 1910 | - | 119 | } |
| 120 | } |
||
| 121 | |||
| 122 | /************************************************************************ |
||
| 123 | * Main procedure. |
||
| 124 | ************************************************************************/ |
||
| 125 | void calculateFlightAttitude(void) { |
||
| 126 | getAnalogData(); |
||
| 127 | integrate(); |
||
| 128 | |||
| 2099 | - | 129 | // We are done reading variables from the analog module. |
| 130 | // Interrupt-driven sensor reading may restart. |
||
| 131 | startAnalogConversionCycle(); |
||
| 1910 | - | 132 | } |