Subversion Repositories FlightCtrl

#include <stdlib.h>
#include <avr/io.h>
#include <math.h>
//#include "eeprom.h"
#include "flight.h"
#include "output.h"
//#include "uart0.h"

// Necessary for external control and motor test
//#include "twimaster.h"
#include "attitude.h"
#include "analog.h"
#include "controlMixer.h"
#include "commands.h"
#include "heightControl.h"
#include "definitions.h"
#include "debug.h"

#ifdef USE_MK3MAG
#include "mk3mag.h"
#include "compassControl.h"
#endif

int16_t targetHeading;
int32_t IPart[2];
FlightMode_t flight_flightMode = FM_UNINITALIZED;
int16_t minThrottle, maxThrottle;

/************************************************************************/
/*  Filter for motor value smoothing (necessary???)                     */
/************************************************************************/
/*
 int16_t motorFilter(int16_t newvalue, int16_t oldvalue) {
 switch (staticParams.motorSmoothing) {
 case 0:
 return newvalue;
 case 1:
 return (oldvalue + newvalue) / 2;
 case 2:
 if (newvalue > oldvalue)
 return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
 else
 return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
 case 3:
 if (newvalue < oldvalue)
 return (1 * (int16_t) oldvalue + newvalue) / 2; //mean of old and new
 else
 return newvalue - (oldvalue - newvalue) * 1; // 2 * new - old
 default:
 return newvalue;
 }
 }
 */

void resetIParts(void) {
  IPart[X] = IPart[Y] = 0;
}

void flight_setMode(FlightMode_t _flightMode) {
  if (flight_flightMode != _flightMode) {
    resetIParts();
    flight_flightMode = _flightMode;
  }
}

// To be called only when necessary.
void flight_setParameters(void) {
  minThrottle = staticParams.minThrottle << LOG_CONTROL_BYTE_SCALING;
  maxThrottle = staticParams.maxThrottle << LOG_CONTROL_BYTE_SCALING;
}

// A heuristic when the yaw attitude cannot be used for yaw control because the airframe is near-vertical or inverted.
// Fum of abs(pitch) and abs(roll) greater than 75 degrees or about 1.3 radians.
uint8_t isHeadingUnaccountable(void) {
  int16_t x = attitude[X];
  if (x<0) x = -x;
  int16_t y = attitude[Y];
  if (y<0) y = -y;
  int32_t result = (int32_t)x + y;
  return result >= (long)(1.3 * INT16DEG_PI_FACTOR);
}

void flight_setGround(void) {
  resetIParts();
  targetHeading = attitude[Z];
}

void flight_takeOff(void) {
  // This is for GPS module to mark home position.
  // TODO: What a disgrace, change it.
  // MKFlags |= MKFLAG_CALIBRATE;
  HC_setGround();
#ifdef USE_MK3MAG
  attitude_resetHeadingToMagnetic();
  compass_setTakeoffHeading(attitude[YAW]);
#endif
}

/************************************************************************/
/*  Main Flight Control                                                 */
/************************************************************************/
void flight_control(void) {

  // PID controller variables
  float PID;

  // High resolution motor values for smoothing of PID motor outputs
  // static int16_t motorFilters[MAX_MOTORS];

  throttleTerm = controls[CONTROL_THROTTLE]; // in the -1000 to 1000 range there about.

  if (throttleTerm > 200 && (MKFlags & MKFLAG_MOTOR_RUN)) {
    // increment flight-time counter until overflow.
    if (isFlying != 0xFFFF)
      isFlying++;
  }
  /*
   * When standing on the ground, do not apply I controls and zero the yaw stick.
   * Probably to avoid integration effects that will cause the copter to spin
   * or flip when taking off.
   */
  if (isFlying < 256) {
    flight_setGround();
    if (isFlying == 255)
      flight_takeOff();
  }

  // This check removed. Is done on a per-motor basis, after output matrix multiplication.
  if (throttleTerm < minThrottle)
    throttleTerm = minThrottle;
  else if (throttleTerm > maxThrottle)
    throttleTerm = maxThrottle;

  // This is where control affects the target heading. It also (later) directly controls yaw.
  targetHeading -= ((int32_t)controls[CONTROL_YAW] * YAW_STICK_INTEGRATION_SCALER_LSHIFT16) >> 16;
  int16_t headingError;

  if (isHeadingUnaccountable()) {
    // inverted flight. Attitude[Z] is off by PI and we should react in the oppisite direction!
    debugOut.digital[0] |= DEBUG_INVERTED;
    headingError = 0;
  } else {
    debugOut.digital[0] &= ~DEBUG_INVERTED;
    headingError = attitude[Z] - targetHeading;
  }

  // Ahaa. Wait. Here is pretty much same check.
  if (headingError < -YAW_I_LIMIT) {
    headingError = -YAW_I_LIMIT;
    targetHeading = attitude[Z] + YAW_I_LIMIT;
  } else if (headingError > YAW_I_LIMIT) {
    headingError = YAW_I_LIMIT;
    targetHeading = attitude[Z] - YAW_I_LIMIT;
  }
 
  //debugOut.analog[24] = targetHeading;
  //debugOut.analog[25] = attitude[Z];
  //debugOut.analog[26] = headingError;
  //debugOut.analog[27] = positiveDynamic;
  //debugOut.analog[28] = negativeDynamic;

  // Yaw P part.
  PID = ((int32_t)gyro_control[Z] * YAW_RATE_SCALER_LSHIFT16 * dynamicParams.yawGyroP) >> 16;

  if (flight_flightMode != FM_RATE) {
    PID += ((int32_t)headingError * ATT_P_SCALER_LSHIFT16 * dynamicParams.yawGyroI) >> 16;
  }

  yawTerm = PID + controls[CONTROL_YAW];
  // yawTerm = limitYawToThrottle(yawTerm);

  /************************************************************************/
  /* Calculate control feedback from angle (gyro integral)                */
  /* and angular velocity (gyro signal)                                   */
  /************************************************************************/
  for (uint8_t axis = CONTROL_ROLL; axis <= CONTROL_PITCH; axis++) {
    if (flight_flightMode == FM_RETURN_TO_LEVEL) {
      // Control.
      // The P part is attitude error.
      PID = (((int32_t)attitude[axis] * ATT_P_SCALER_LSHIFT16 * dynamicParams.attGyroP) >> 16) + controls[axis];
      // The I part is attitude error integral.
      IPart[axis] += PID;
      // The D part is rate.
      PID += ((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.attGyroD) >> 16;
    } else {
      // We want: Normal stick gain, full stick deflection should drive gyros halfway towards saturation.
      // If that is not enough, then fully towards saturation.
      PID = (((int32_t)gyro_control[axis] * RATE_P_SCALER_LSHIFT16 * dynamicParams.rateGyroP) >> 16) + controls[axis];
      IPart[axis] += PID;
      PID -= ((int32_t)gyroD[axis] * dynamicParams.rateGyroD) >> 8;
    }

    // PDPart += (gyroD[axis] * (int16_t) dynamicParams.gyroD) / 16;
    // Right now, let us ignore I.
    // term[axis] = PDPart - controls[axis] + (((int32_t) IPart[axis] * ki) >> 14);
    term[axis] = PID;
    term[axis] += (dynamicParams.levelCorrection[axis] - 128);

    debugOut.analog[12 + axis] = term[axis];
  }

  //debugOut.analog[14] = yawTerm;
  //debugOut.analog[15] = throttleTerm;
  //debugOut.analog[15] = IPart[0] / 1000;

  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
  // Universal Mixer
  // Each (pitch, roll, throttle, yaw) term is in the range [0..255 * CONTROL_SCALING].
  // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

  debugOut.analog[9] = controls[CONTROL_PITCH];
  debugOut.analog[10] = controls[CONTROL_YAW];
  debugOut.analog[11] = controls[CONTROL_THROTTLE];
}