Subversion Repositories FlightCtrl

#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.
 */
int16_t rate_ATT[3];

// With different (less) filtering
int16_t rate_PID[3];
int16_t differential[3];

/*
 * 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++) {
    rate_PID[axis] = gyro_PID[axis];
    rate_ATT[axis] = gyro_ATT[axis];
    differential[axis] = gyroD[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] += rate_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();
}