WebSVN - FlightCtrl - Diff - Rev 1796 and 1821 - /branches/dongfang_FC

 // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
 // + 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 "heightControl.h"
 #include "attitudeControl.h"
 #include "externalControl.h"
 #include "configuration.h"
 #include "attitude.h"
 #include "commands.h"
 #include "output.h"
-uint16_t maxControl[2] = {0,0};
+uint16_t maxControl[2] = { 0, 0 };
-int16_t control[2] = {0,0};
+int16_t control[2] = { 0, 0 };
 int16_t 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 lastCommand = COMMAND_NONE;
 uint8_t lastArgument;
 uint8_t isCommandRepeated = 0;
 // MK flags. TODO: Replace by enum. State machine.
 uint16_t isFlying = 0;
 volatile uint8_t MKFlags = 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 lastArgument;
+        return lastArgument;
+}
 /*
  * 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) {
-  return lastCommand;
+        return lastCommand;
+}
 uint8_t controlMixer_isCommandRepeated(void) {
-  return isCommandRepeated;
+        return isCommandRepeated;
+}
 void controlMixer_setNeutral() {
-  EC_setNeutral();
+        EC_setNeutral();
-  HC_setGround();
+        HC_setGround();
+}
 /*
  * 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;
+        uint8_t i;
-  for (i=0; i<8; i++) {
+        for (i = 0; i < 8; i++) {
-    variables[i] = RC_getVariable(i);
+                variables[i] = RC_getVariable(i);
-  }
+        }
+}
 /*
  * Update potentiometer values with limited slew rate. Could be made faster if desired.
  * TODO: It assumes R/C as source. Not necessarily true.
  */
 void controlMixer_updateVariables(void) {
-  uint8_t i;
+        uint8_t i;
-  int16_t targetvalue;
+        int16_t targetvalue;
-  for (i=0; i<8; i++) {
+        for (i = 0; i < 8; i++) {
-    targetvalue = RC_getVariable(i);
+                targetvalue = RC_getVariable(i);
-    if (targetvalue < 0) targetvalue = 0;
+                if (targetvalue < 0)
+                        targetvalue = 0;
+                if (variables[i] < targetvalue && variables[i] < 255)
+                        variables[i]++;
-    if (variables[i] < targetvalue && variables[i] < 255) variables[i]++; else if(variables[i] > 0 && variables[i] > targetvalue) variables[i]--;
+                else if (variables[i] > 0 && variables[i] > targetvalue)
+                        variables[i]--;
-  }
+        }
+}
 uint8_t controlMixer_getSignalQuality(void) {
-  uint8_t rcQ = RC_getSignalQuality();
+        uint8_t rcQ = RC_getSignalQuality();
-  uint8_t ecQ = EC_getSignalQuality();
+        uint8_t ecQ = EC_getSignalQuality();
-  // This needs not be the only correct solution...
+        // This needs not be the only correct solution...
-  return rcQ > ecQ ? rcQ : ecQ;
+        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)
+        // calculate Stick inputs by rc channels (P) and changing of rc channels (D)
-  // TODO: If no signal --> zero.
+        // TODO: If no signal --> zero.
-  uint8_t axis;
+        uint8_t axis;
-  // takes almost no time...
+        // takes almost no time...
-  RC_update();
+        RC_update();
-  // takes almost no time...
+        // takes almost no time...
-  EC_update();
+        EC_update();
-  // takes about 80 usec.
+        // takes about 80 usec.
-  HC_update();
+        HC_update();
-  int16_t* RC_PRTY = RC_getPRTY();
+        int16_t* RC_PRTY = RC_getPRTY();
-  int16_t* EC_PRTY = EC_getPRTY();
+        int16_t* EC_PRTY = EC_getPRTY();
-  control[PITCH]   = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
+        control[PITCH] = RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH];
-  control[ROLL]    = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
+        control[ROLL] = RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL];
-  // This can be a CPU time killer if the function implementations are inefficient.
+        // This can be a CPU time killer if the function implementations are inefficient.
-  controlThrottle  = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]));
+        controlThrottle = HC_getThrottle(AC_getThrottle(RC_PRTY[CONTROL_THROTTLE]
+                        + EC_PRTY[CONTROL_THROTTLE]));
-  controlYaw       = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
+        controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];
-  DebugOut.Analog[12] = control[PITCH];
+        DebugOut.Analog[12] = control[PITCH];
-  DebugOut.Analog[13] = control[ROLL];
+        DebugOut.Analog[13] = control[ROLL];
-  //DebugOut.Analog[26] = controlYaw;
+        //DebugOut.Analog[26] = controlYaw;
-  if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
+        if (controlMixer_getSignalQuality() >= SIGNAL_GOOD) {
-    controlMixer_updateVariables();
+                controlMixer_updateVariables();
-    configuration_applyVariablesToParams();
+                configuration_applyVariablesToParams();
-    looping = RC_getLooping(looping);
+                looping = RC_getLooping(looping);
-  } else { // Signal is not OK
+        } else { // Signal is not OK
-    // Could handle switch to emergency flight here.
+                // Could handle switch to emergency flight here.
-    // throttle is handled elsewhere.
+                // throttle is handled elsewhere.
-    looping = 0;
+                looping = 0;
-  }
+        }
-  // part1a end.
+        // part1a end.
-  /* This is not really necessary with the dead-gap feature on all sticks (see rc.c)
+        /* This is not really necessary with the dead-gap feature on all sticks (see rc.c)
-  if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
+         if(staticParams.GlobalConfig & (CFG_COMPASS_ACTIVE | CFG_GPS_ACTIVE)) {
-    if (controlYaw > 2) controlYaw-= 2;
+         if (controlYaw > 2) controlYaw-= 2;
-    else if (controlYaw< -2) controlYaw += 2;
+         else if (controlYaw< -2) controlYaw += 2;
-    else controlYaw = 0;
+         else controlYaw = 0;
-  }
+         }
-  */
+         */
-  /*
+        /*
-   * Record maxima
+         * Record maxima
-   */
+         */
-  for (axis=PITCH; axis<=ROLL; axis++) {
+        for (axis = PITCH; axis <= ROLL; axis++) {
-    if(abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
+                if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {
-      maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
+                        maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;
-      if(maxControl[axis] > 100) maxControl[axis] = 100;
+                        if (maxControl[axis] > 100)
+                                maxControl[axis] = 100;
-    } else if (maxControl[axis]) maxControl[axis]--;
+                } else if (maxControl[axis])
+                        maxControl[axis]--;
-  }
+        }
-  uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand() : COMMAND_NONE;
+        uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()
+                        : COMMAND_NONE;
-  uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand() : COMMAND_NONE;
+        uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()
+                        : COMMAND_NONE;
-  if (rcCommand != COMMAND_NONE) {
+        if (rcCommand != COMMAND_NONE) {
-    isCommandRepeated = (lastCommand == rcCommand);
+                isCommandRepeated = (lastCommand == rcCommand);
-    lastCommand = rcCommand;
+                lastCommand = rcCommand;
-    lastArgument = RC_getArgument();
+                lastArgument = RC_getArgument();
-  } else if (ecCommand != COMMAND_NONE) {
+        } else if (ecCommand != COMMAND_NONE) {
-    isCommandRepeated = (lastCommand == ecCommand);
+                isCommandRepeated = (lastCommand == ecCommand);
-    lastCommand = ecCommand;
+                lastCommand = ecCommand;
-    lastArgument = EC_getArgument();
+                lastArgument = EC_getArgument();
-  } else {
+        } else {
-    // Both sources have no command, or one or both are out.
+                // Both sources have no command, or one or both are out.
-    // Just set to false. There is no reason to check if the none-command was repeated anyway.
+                // Just set to false. There is no reason to check if the none-command was repeated anyway.
-    isCommandRepeated = 0;
+                isCommandRepeated = 0;
-    lastCommand = COMMAND_NONE;
+                lastCommand = COMMAND_NONE;
-  }
+        }
+        if (isCommandRepeated)
+                DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED;
+        else
+                DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
+        if (rcCommand)
+                DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED;
+        else
+                DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED;
-  if (isCommandRepeated) DebugOut.Digital[0] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[0] &= ~DEBUG_COMMANDREPEATED;
-  if (rcCommand) DebugOut.Digital[1] |= DEBUG_COMMANDREPEATED; else DebugOut.Digital[1] &= ~DEBUG_COMMANDREPEATED;
-  // part1 end.
+        // part1 end.
+}
 // TODO: Integrate into command system.
 uint8_t controlMixer_testCompassCalState(void) {
-  return RC_testCompassCalState();
+        return RC_testCompassCalState();
+}

Subversion Repositories FlightCtrl

(root)/branches/dongfang_FC_rewrite/controlMixer.c @ 1965 - Rev 1796 → 1821