Subversion Repositories FlightCtrl

// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Copyright (c) 04.2007 Holger Buss
// + Nur für den privaten Gebrauch
// + www.MikroKopter.com
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation),
// + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist.
// + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt
// + bzgl. der Nutzungsbedingungen aufzunehmen.
// + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen,
// + Verkauf von Luftbildaufnahmen, usw.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht,
// + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts
// + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de"
// + eindeutig als Ursprung verlinkt werden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion
// + Benutzung auf eigene Gefahr
// + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur
// + mit unserer Zustimmung zulässig
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
// + Redistributions of source code (with or without modifications) must retain the above copyright notice,
// + this list of conditions and the following disclaimer.
// +   * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived
// +     from this software without specific prior written permission.
// +   * The use of this project (hardware, software, binary files, sources and documentation) is only permittet
// +     for non-commercial use (directly or indirectly)
// +     Commercial use (for excample: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted
// +     with our written permission
// +   * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be
// +     clearly linked as origin
// +   * porting to systems other than hardware from www.mikrokopter.de is not allowed
// +  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// +  AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// +  IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// +  ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// +  LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// +  CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// +  SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// +  INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// +  CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// +  ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// +  POSSIBILITY OF SUCH DAMAGE.
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++

#include <stdlib.h>
#include "controlMixer.h"
#include "rc.h"
#include "externalControl.h"
#include "configuration.h"
#include "attitude.h"
#include "eeprom.h"
#include "flight.h"

/*
 * Number of cycles a command must be repeated before commit. Maybe this really belongs in RC.
 */
#define COMMAND_TIMER 200

uint16_t maxControlPitch = 0, maxControlRoll = 0;
int16_t controlPitch = 0, controlRoll = 0, controlYaw = 0, controlThrottle = 0;
uint8_t looping = 0;

// Internal variables for reading commands made with an R/C stick.
uint8_t lastCommand  = COMMAND_NONE;
uint8_t commandTimer = 0;

/*
 * This could be expanded to take arguments from ohter sources than the RC
 * (read: Custom MK RC project)
 */
uint8_t controlMixer_getArgument(void) {
  return RC_getArgument();
}

/*
 * This could be expanded to take calibrate / start / stop commands from ohter sources
 * than the R/C (read: Custom MK R/C project)
 */
uint8_t controlMixer_getCommand(void) {
  // If the same command was made COMMAND_TIMER times, it's stable.
  if (commandTimer >= COMMAND_TIMER) {
    return lastCommand;
  }
  return COMMAND_NONE;
}

uint8_t controlMixer_isCommandRepeated(void) {
  return commandTimer > COMMAND_TIMER ? 1 : 0;
}

void controlMixer_setNeutral(uint8_t calibrate) {
  if (calibrate)
    RC_calibrate();
  EC_setNeutral();
}

/*
 * Set the potientiometer values to the momentary values of the respective R/C channels.
 * No slew rate limitation.
 */
void controlMixer_initVariables(void) {
  uint8_t i;
  for (i=0; i<8; i++) {
    variables[i] = RC_getVariable(i);
  }
}

/*
 * Update potentiometer values with limited slew rate. Could be made faster if desired.
 */
void controlMixer_updateVariables(void) {
  uint8_t i;
  int16_t targetvalue;
  for (i=0; i<8; i++) {
    targetvalue = RC_getVariable(i);
    if (targetvalue < 0) targetvalue = 0;
    if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;
  }
}

uint8_t controlMixer_getSignalQuality(void) {
  uint8_t rcQ = RC_getSignalQuality();
  uint8_t ecQ = EC_getSignalQuality();
  DebugOut.Analog[16] = rcQ;
  DebugOut.Analog[17] = ecQ;
  // This needs not be the only correct solution...
  return rcQ > ecQ ? rcQ : ecQ;
}

/*
 * Update the variables indicating stick position from the sum of R/C, GPS and external control.
 */
void controlMixer_update(void) {
  // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
  // TODO: If no signal --> zero.
  RC_update();
  EC_update();

  int16_t* RC_PRTY = RC_getPRTY();
  int16_t* EC_PRTY = EC_getPRTY();

  controlPitch     = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
  controlRoll      = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
  controlThrottle  = RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE];
  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];

  DebugOut.Analog[6] = controlPitch;
  DebugOut.Analog[7] = controlRoll;
  DebugOut.Analog[8] = controlYaw;
   
  if (RC_getSignalQuality() >= SIGNAL_GOOD) {
    controlMixer_updateVariables();
    configuration_applyVariablesToParams();
    looping = RC_getLooping(looping);
  } else { // Signal is not OK
    // Could handle switch to emergency flight here.
    // throttle is handled elsewhere.
    looping = 0;
  }

  DebugOut.Analog[18] = (int16_t)looping;

  // (range of -2 .. 2 is set to zero, to avoid unwanted yaw trimming on compass correction)
  // TODO: Correct, for changed range (stick gain + expo is now applied already)
  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
    if (controlYaw > 2) controlYaw-= 2;
    else if (controlYaw< -2) controlYaw += 2;
    else controlYaw = 0;
  }

  /*
   * Record maxima
   */
  if(abs(controlPitch / STICK_GAIN) > maxControlPitch) {
    maxControlPitch = abs(controlPitch) / STICK_GAIN;
    if(maxControlPitch > 100) maxControlPitch = 100;
  } else if (maxControlPitch) maxControlPitch--;
  if(abs(controlRoll / STICK_GAIN) > maxControlRoll) {
    maxControlRoll = abs(controlRoll) / STICK_GAIN;
    if(maxControlRoll > 100) maxControlRoll = 100;
  }
  else if (maxControlRoll) maxControlRoll--;

  // Here we could blend in other sources.
  uint8_t commandNow = RC_getCommand();

  if (commandNow != lastCommand) {
    lastCommand = commandNow;
    commandTimer = 0;
  } else if (commandTimer < COMMAND_TIMER + 1)
    commandTimer++;
}

/*
void setCompassCalState(void) {
  static uint8_t stick = 1;
  // if pitch is centered or top set stick to zero
  if(RCChannel(CH_PITCH) > -20) stick = 0;
  // if pitch is down trigger to next cal state
  if((RCChannel(CH_PITCH) < -70) && !stick) {
    stick = 1;
    compassCalState++;
    if(compassCalState < 5) beepNumber(compassCalState);
    else beep(1000);
  }
}
*/