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 example: 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 controlActivity = 0;

int16_t controls[4] = { 0, 0, 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 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;

}

/*

 * 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;

}

uint8_t controlMixer_isCommandRepeated(void) {

        return isCommandRepeated;

}

void controlMixer_setNeutral() {

        EC_setNeutral();

        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;

        for (i = 0; i < 8; 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;

        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();

        // This needs not be the only correct solution...

        return rcQ > ecQ ? rcQ : ecQ;

}

void updateControlAndMeasureControlActivity(uint8_t index, int16_t newValue) {

    int16_t tmp = controls[index];

    controls[index] = newValue;

    tmp -= newValue;

    tmp /= 2;

    tmp = tmp * tmp;

    // tmp += (newValue >= 0) ? newValue : -newValue;

    if (controlActivity + (uint16_t)tmp >= controlActivity)

      controlActivity += tmp;

    else controlActivity = 0xffff;

}

#define CADAMPING 10

void dampenControlActivity(void) {

    uint32_t tmp = controlActivity;

    tmp *= ((1<<CADAMPING)-1);

    tmp >>= CADAMPING;

    controlActivity = tmp;

}

/*

 * 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.

        int16_t tempThrottle;

        // takes almost no time...

        RC_update();

        // takes almost no time...

        EC_update();

        // takes about 80 usec.

        HC_update();

    int16_t* RC_PRTY = RC_getPRTY();

    int16_t* EC_PRTY = EC_getPRTY();

    updateControlAndMeasureControlActivity(CONTROL_PITCH, RC_PRTY[CONTROL_PITCH] + EC_PRTY[CONTROL_PITCH]);

    updateControlAndMeasureControlActivity(CONTROL_ROLL, RC_PRTY[CONTROL_ROLL] + EC_PRTY[CONTROL_ROLL]);

    updateControlAndMeasureControlActivity(CONTROL_YAW, RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW]);

    dampenControlActivity();

    DebugOut.Analog[14] = controlActivity/10;

        tempThrottle = HC_getThrottle(RC_PRTY[CONTROL_THROTTLE] + EC_PRTY[CONTROL_THROTTLE]);

        controls[CONTROL_THROTTLE] = AC_getThrottle(tempThrottle);

        // controlYaw = RC_PRTY[CONTROL_YAW] + EC_PRTY[CONTROL_YAW];

        if (controlMixer_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;

        }

        // part1a end.

        /* This is not really necessary with the dead-band feature on all sticks (see rc.c)

         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

        for (axis = PITCH; axis <= ROLL; axis++) {

                if (abs(control[axis] / CONTROL_SCALING) > maxControl[axis]) {

                        maxControl[axis] = abs(control[axis]) / CONTROL_SCALING;

                        if (maxControl[axis] > 100)

                                maxControl[axis] = 100;

                } else if (maxControl[axis])

                        maxControl[axis]--;

        }

    */

        uint8_t rcCommand = (RC_getSignalQuality() >= SIGNAL_OK) ? RC_getCommand()

                        : COMMAND_NONE;

        uint8_t ecCommand = (EC_getSignalQuality() >= SIGNAL_OK) ? EC_getCommand()

                        : COMMAND_NONE;

        if (rcCommand != COMMAND_NONE) {

                isCommandRepeated = (lastCommand == rcCommand);

                lastCommand = rcCommand;

                lastArgument = RC_getArgument();

        } else if (ecCommand != COMMAND_NONE) {

                isCommandRepeated = (lastCommand == ecCommand);

                lastCommand = ecCommand;

                lastArgument = EC_getArgument();

        } else {

                // 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.

                isCommandRepeated = 0;

                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;

        */

}

// TODO: Integrate into command system.

uint8_t controlMixer_testCompassCalState(void) {

        return RC_testCompassCalState();

}