| 0,0 → 1,122 |
|---|
| #include <stdlib.h> |
| #include <avr/io.h> |
| #include <stdlib.h> |
| |
| #include "attitude.h" |
| #include "dongfangMath.h" |
| #include "commands.h" |
| |
| // For scope debugging only! |
| #include "rc.h" |
| |
| // where our main data flow comes from. |
| #include "analog.h" |
| |
| #include "configuration.h" |
| #include "output.h" |
| |
| // Some calculations are performed depending on some stick related things. |
| #include "controlMixer.h" |
| |
| #define CHECK_MIN_MAX(value, min, max) {if(value < min) value = min; else if(value > max) value = max;} |
| |
| /* |
| * Gyro readings, as read from the analog module. It would have been nice to flow |
| * them around between the different calculations as a struct or array (doing |
| * things functionally without side effects) but this is shorter and probably |
| * faster too. |
| * The variables are overwritten at each attitude calculation invocation - the values |
| * are not preserved or reused. |
| */ |
| |
| /* |
| * Gyro integrals. These are the rotation angles of the airframe compared to the |
| * horizontal plane, yaw relative to yaw at start. |
| */ |
| int32_t attitude[3]; |
| |
| /* |
| * Experiment: Compensating for dynamic-induced gyro biasing. |
| */ |
| int16_t driftComp[3] = { 0, 0, 0 }; |
| // int16_t savedDynamicOffsetPitch = 0, savedDynamicOffsetRoll = 0; |
| // int32_t dynamicCalPitch, dynamicCalRoll, dynamicCalYaw; |
| // int16_t dynamicCalCount; |
| |
| /************************************************************************ |
| * Set inclination angles from the acc. sensor data. |
| * If acc. sensors are not used, set to zero. |
| * TODO: One could use inverse sine to calculate the angles more |
| * accurately, but since: 1) the angles are rather small at times when |
| * it makes sense to set the integrals (standing on ground, or flying at |
| * constant speed, and 2) at small angles a, sin(a) ~= constant * a, |
| * it is hardly worth the trouble. |
| ************************************************************************/ |
| /* |
| int32_t getAngleEstimateFromAcc(uint8_t axis) { |
| return (int32_t) GYRO_ACC_FACTOR * (int32_t) filteredAcc[axis]; |
| } |
| */ |
| |
| void setStaticAttitudeAngles(void) { |
| attitude[PITCH] = attitude[ROLL] = 0; |
| } |
| |
| /************************************************************************ |
| * Neutral Readings |
| ************************************************************************/ |
| void attitude_setNeutral(void) { |
| // Calibrate hardware. |
| analog_setNeutral(); |
| |
| // reset gyro integrals to acc guessing |
| setStaticAttitudeAngles(); |
| } |
| |
| /************************************************************************ |
| * Get sensor data from the analog module, and release the ADC |
| * TODO: Ultimately, the analog module could do this (instead of dumping |
| * the values into variables). |
| * The rate variable end up in a range of about [-1024, 1023]. |
| *************************************************************************/ |
| void getAnalogData(void) { |
| uint8_t axis; |
| analog_update(); |
| for (axis = PITCH; axis <= YAW; axis++) { |
| } |
| } |
| |
| void integrate(void) { |
| // First, perform axis coupling. If disabled xxxRate is just copied to ACxxxRate. |
| uint8_t axis; |
| |
| /* |
| * Yaw |
| * Calculate yaw gyro integral (~ to rotation angle) |
| * Limit heading proportional to 0 deg to 360 deg |
| */ |
| /* |
| * Pitch axis integration and range boundary wrap. |
| */ |
| |
| for (axis = PITCH; axis <= YAW; axis++) { |
| attitude[axis] += gyro_ATT[axis]; |
| if (attitude[axis] > OVER180) { |
| attitude[axis] -= OVER360; |
| } else if (attitude[axis] <= -OVER180) { |
| attitude[axis] += OVER360; |
| } |
| } |
| } |
| |
| /************************************************************************ |
| * Main procedure. |
| ************************************************************************/ |
| void calculateFlightAttitude(void) { |
| getAnalogData(); |
| integrate(); |
| |
| // We are done reading variables from the analog module. |
| // Interrupt-driven sensor reading may restart. |
| startAnalogConversionCycle(); |
| } |