Rev 2110 |
Blame |
Compare with Previous |
Last modification |
View Log
| RSS feed
#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] = attitude[YAW] = 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) {
analog_update();
}
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();
}